From f7518203aac9762cd24736fe7ddaed65d91aeb19 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 4 Dec 2016 15:48:15 +1100 Subject: [PATCH 1/3] Initial rework - removing target specific code from main.c Moved FLASH to masterConfig Moved LED (named statusLeds) to masterConfig Fixed up some targets to remove defines from main.c --- src/main/config/config_master.h | 10 ++ src/main/drivers/flash.h | 5 + src/main/drivers/flash_m25p16.c | 14 +-- src/main/drivers/flash_m25p16.h | 4 +- src/main/drivers/light_led.c | 85 +++-------------- src/main/drivers/light_led.h | 9 +- src/main/fc/config.c | 46 ++++++++++ src/main/main.c | 92 ++++++------------- src/main/target/ALIENFLIGHTF3/config.c | 28 ++++++ .../target/ALIENFLIGHTF3/initialisation.c | 48 ++++++++++ src/main/target/ALIENFLIGHTF3/target.h | 1 + src/main/target/BLUEJAYF4/hardware_revision.c | 3 +- src/main/target/CJMCU/initialisation.c | 36 ++++++++ src/main/target/CJMCU/target.h | 1 + src/main/target/NAZE/config.c | 5 +- src/main/target/NAZE/hardware_revision.c | 1 + src/main/target/NAZE/initialisation.c | 44 +++++++++ src/main/target/NAZE/target.h | 10 +- src/main/target/SPRACINGF3/target.h | 1 + src/main/target/SPRACINGF3MINI/target.h | 8 +- src/main/target/X_RACERSPI/target.h | 2 +- 21 files changed, 288 insertions(+), 165 deletions(-) create mode 100644 src/main/target/ALIENFLIGHTF3/initialisation.c create mode 100644 src/main/target/CJMCU/initialisation.c create mode 100644 src/main/target/NAZE/initialisation.c diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index cc30a6974..338f56df8 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -31,6 +31,8 @@ #include "drivers/sonar_hcsr04.h" #include "drivers/sdcard.h" #include "drivers/vcd.h" +#include "drivers/light_led.h" +#include "drivers/flash.h" #include "fc/rc_controls.h" @@ -90,10 +92,12 @@ #define beeperConfig(x) (&masterConfig.beeperConfig) #define sonarConfig(x) (&masterConfig.sonarConfig) #define ledStripConfig(x) (&masterConfig.ledStripConfig) +#define statusLedConfig(x) (&masterConfig.statusLedConfig) #define osdProfile(x) (&masterConfig.osdProfile) #define vcdProfile(x) (&masterConfig.vcdProfile) #define sdcardConfig(x) (&masterConfig.sdcardConfig) #define blackboxConfig(x) (&masterConfig.blackboxConfig) +#define flashConfig(x) (&masterConfig.flashConfig) // System-wide @@ -162,6 +166,8 @@ typedef struct master_s { serialConfig_t serialConfig; telemetryConfig_t telemetryConfig; + statusLedConfig_t statusLedConfig; + #ifdef USE_PPM ppmConfig_t ppmConfig; #endif @@ -226,6 +232,10 @@ typedef struct master_s { blackboxConfig_t blackboxConfig; #endif +#ifdef USE_FLASHFS + flashConfig_t flashConfig; +#endif + uint32_t beeper_off_flags; uint32_t preferred_beeper_off_flags; diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h index 21216825c..6a6f160b0 100644 --- a/src/main/drivers/flash.h +++ b/src/main/drivers/flash.h @@ -18,6 +18,7 @@ #pragma once #include +#include "drivers/io_types.h" typedef struct flashGeometry_s { uint16_t sectors; // Count of the number of erasable blocks on the device @@ -29,3 +30,7 @@ typedef struct flashGeometry_s { uint32_t totalSize; // This is just sectorSize * sectors } flashGeometry_t; + +typedef struct flashConfig_s { + ioTag_t csTag; +} flashConfig_t; diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 67219ce30..c6429fe43 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -208,27 +208,21 @@ static bool m25p16_readIdentification() * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with * m25p16_getGeometry(). */ -bool m25p16_init(ioTag_t csTag) +bool m25p16_init(flashConfig_t *flashConfig) { /* if we have already detected a flash device we can simply exit - - TODO: change the init param in favour of flash CFG when ParamGroups work is done - then cs pin can be specified in hardware_revision.c or config.c (dependent on revision). */ if (geometry.sectors) { return true; } - if (csTag) { - m25p16CsPin = IOGetByTag(csTag); + if (flashConfig->csTag) { + m25p16CsPin = IOGetByTag(flashConfig->csTag); } else { -#ifdef M25P16_CS_PIN - m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); -#else return false; -#endif } + IOInit(m25p16CsPin, OWNER_FLASH_CS, 0); IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG); diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index ace78fc8b..f3cec0d9f 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -18,11 +18,11 @@ #pragma once #include -#include "io_types.h" +#include "flash.h" #define M25P16_PAGESIZE 256 -bool m25p16_init(ioTag_t csTag); +bool m25p16_init(flashConfig_t *flashConfig); void m25p16_eraseSector(uint32_t address); void m25p16_eraseCompletely(); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 1c9271883..8011f04cf 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -22,82 +22,23 @@ #include "light_led.h" -static const IO_t leds[] = { -#ifdef LED0 - DEFIO_IO(LED0), -#else - DEFIO_IO(NONE), -#endif -#ifdef LED1 - DEFIO_IO(LED1), -#else - DEFIO_IO(NONE), -#endif -#ifdef LED2 - DEFIO_IO(LED2), -#else - DEFIO_IO(NONE), -#endif -#if defined(LED0_A) || defined(LED1_A) || defined(LED2_A) -#ifdef LED0_A - DEFIO_IO(LED0_A), -#else - DEFIO_IO(NONE), -#endif -#ifdef LED1_A - DEFIO_IO(LED1_A), -#else - DEFIO_IO(NONE), -#endif -#ifdef LED2_A - DEFIO_IO(LED2_A), -#else - DEFIO_IO(NONE), -#endif -#endif -}; +static IO_t leds[LED_NUMBER]; +static uint8_t ledPolarity = 0; -uint8_t ledPolarity = 0 -#ifdef LED0_INVERTED - | BIT(0) -#endif -#ifdef LED1_INVERTED - | BIT(1) -#endif -#ifdef LED2_INVERTED - | BIT(2) -#endif -#ifdef LED0_A_INVERTED - | BIT(3) -#endif -#ifdef LED1_A_INVERTED - | BIT(4) -#endif -#ifdef LED2_A_INVERTED - | BIT(5) -#endif - ; - -static uint8_t ledOffset = 0; - -void ledInit(bool alternative_led) +void ledInit(statusLedConfig_t *statusLedConfig) { -#if defined(LED0_A) || defined(LED1_A) || defined(LED2_A) - if (alternative_led) { - ledOffset = LED_NUMBER; - } -#else - UNUSED(alternative_led); -#endif - LED0_OFF; LED1_OFF; LED2_OFF; + ledPolarity = statusLedConfig->polarity; for (int i = 0; i < LED_NUMBER; i++) { - if (leds[i + ledOffset]) { - IOInit(leds[i + ledOffset], OWNER_LED, RESOURCE_INDEX(i)); - IOConfigGPIO(leds[i + ledOffset], IOCFG_OUT_PP); + if (statusLedConfig->ledTags[i]) { + leds[i] = IOGetByTag(statusLedConfig->ledTags[i]); + IOInit(leds[i], OWNER_LED, RESOURCE_INDEX(i)); + IOConfigGPIO(leds[i], IOCFG_OUT_PP); + } else { + leds[i] = IO_NONE; } } @@ -108,11 +49,11 @@ void ledInit(bool alternative_led) void ledToggle(int led) { - IOToggle(leds[led + ledOffset]); + IOToggle(leds[led]); } void ledSet(int led, bool on) { - const bool inverted = (1 << (led + ledOffset)) & ledPolarity; - IOWrite(leds[led + ledOffset], on ? inverted : !inverted); + const bool inverted = (1 << (led)) & ledPolarity; + IOWrite(leds[led], on ? inverted : !inverted); } diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 55814e1a1..d1ae47294 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -17,8 +17,15 @@ #pragma once +#include "drivers/io_types.h" + #define LED_NUMBER 3 +typedef struct statusLedConfig_s { + ioTag_t ledTags[LED_NUMBER]; + uint8_t polarity; +} statusLedConfig_t; + // Helpful macros #ifdef LED0 # define LED0_TOGGLE ledToggle(0) @@ -50,6 +57,6 @@ # define LED2_ON do {} while(0) #endif -void ledInit(bool alternative_led); +void ledInit(statusLedConfig_t *statusLedConfig); void ledToggle(int led); void ledSet(int led, bool state); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index fa1cc67cd..59a8ca818 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -490,6 +490,46 @@ void resetMax7456Config(vcdProfile_t *pVcdProfile) } #endif +void resetStatusLedConfig(statusLedConfig_t *statusLedConfig) +{ + for (int i = 0; i < LED_NUMBER; i++) { + statusLedConfig->ledTags[i] = IO_TAG_NONE; + } + +#ifdef LED0 + statusLedConfig->ledTags[0] = IO_TAG(LED0); +#endif +#ifdef LED1 + statusLedConfig->ledTags[1] = IO_TAG(LED1); +#endif +#ifdef LED2 + statusLedConfig->ledTags[2] = IO_TAG(LED2); +#endif + + statusLedConfig->polarity = 0 +#ifdef LED0_INVERTED + | BIT(0) +#endif +#ifdef LED1_INVERTED + | BIT(1) +#endif +#ifdef LED2_INVERTED + | BIT(2) +#endif + ; +} + +#ifdef USE_FLASHFS +void resetFlashConfig(flashConfig_t *flashConfig) +{ +#ifdef M25P16_CS_PIN + flashConfig->csTag = IO_TAG(M25P16_CS_PIN); +#else + flashConfig->csTag = IO_TAG_NONE; +#endif +} +#endif + uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; @@ -789,6 +829,12 @@ void createDefaultConfig(master_t *config) } #endif +#ifdef USE_FLASHFS + resetFlashConfig(&config->flashConfig); +#endif + + resetStatusLedConfig(&config->statusLedConfig); + #if defined(TARGET_CONFIG) targetConfiguration(config); #endif diff --git a/src/main/main.c b/src/main/main.c index cf1060a54..4f080597c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -124,6 +124,10 @@ #include "build/build_config.h" #include "build/debug.h" +#ifdef TARGET_BUS_INIT +void targetBusInit(void); +#endif + extern uint8_t motorControlEnable; #ifdef SOFTSERIAL_LOOPBACK @@ -172,11 +176,7 @@ void init(void) // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); -#ifdef ALIENFLIGHTF3 - ledInit(hardwareRevision == AFF3_REV_1 ? false : true); -#else - ledInit(false); -#endif + ledInit(&masterConfig.statusLedConfig); LED2_ON; #ifdef USE_EXTI @@ -291,7 +291,6 @@ void init(void) pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode); #endif - systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER @@ -306,74 +305,48 @@ void init(void) bstInit(BST_DEVICE); #endif -#ifdef USE_SPI -#ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); -#endif -#ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); -#endif -#ifdef USE_SPI_DEVICE_3 -#ifdef ALIENFLIGHTF3 - if (hardwareRevision == AFF3_REV_2) { - spiInit(SPIDEV_3); - } +#ifdef TARGET_BUS_INIT + targetBusInit(); #else - spiInit(SPIDEV_3); -#endif -#endif -#ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); -#endif -#endif + #ifdef USE_SPI + #ifdef USE_SPI_DEVICE_1 + spiInit(SPIDEV_1); + #endif + #ifdef USE_SPI_DEVICE_2 + spiInit(SPIDEV_2); + #endif + #ifdef USE_SPI_DEVICE_3 + spiInit(SPIDEV_3); + #endif + #ifdef USE_SPI_DEVICE_4 + spiInit(SPIDEV_4); + #endif + #endif -#ifdef VTX - vtxInit(); + #ifdef USE_I2C + i2cInit(I2C_DEVICE); + #endif #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif -#if defined(NAZE) - if (hardwareRevision == NAZE32_SP) { - serialRemovePort(SERIAL_PORT_SOFTSERIAL2); - } else { - serialRemovePort(SERIAL_PORT_USART3); - } +#ifdef VTX + vtxInit(); #endif -#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) +#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif -#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI) -#if defined(SONAR) && defined(USE_SOFTSERIAL1) +#if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif -#endif - -#ifdef USE_I2C -#if defined(NAZE) - if (hardwareRevision != NAZE32_SP) { - i2cInit(I2C_DEVICE); - } else { - if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { - i2cInit(I2C_DEVICE); - } - } -#elif defined(CC3D) - if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { - i2cInit(I2C_DEVICE); - } -#else - i2cInit(I2C_DEVICE); -#endif -#endif #ifdef USE_ADC /* these can be removed from features! */ @@ -383,7 +356,6 @@ void init(void) adcInit(&masterConfig.adcConfig); #endif - initBoardAlignment(&masterConfig.boardAlignment); #ifdef CMS @@ -513,12 +485,8 @@ void init(void) #endif #ifdef USE_FLASHFS -#ifdef NAZE - if (hardwareRevision == NAZE32_REV5) { - m25p16_init(IO_TAG_NONE); - } -#elif defined(USE_FLASH_M25P16) - m25p16_init(IO_TAG_NONE); +#if defined(USE_FLASH_M25P16) + m25p16_init(&masterConfig.flashConfig); #endif flashfsInit(); diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 516b64a78..4bf94258b 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -45,6 +45,34 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(master_t *config) { + /* depending on revision ... depends on the LEDs to be utilised. */ + if (hardwareRevision == AFF3_REV_1) { + config->statusLedConfig.polarity = 0 +#ifdef LED0_A_INVERTED + | BIT(3) +#endif +#ifdef LED1_A_INVERTED + | BIT(4) +#endif +#ifdef LED2_A_INVERTED + | BIT(5) +#endif + ; + + for (int i = 0; i < LED_NUMBER; i++) { + config->statusLedConfig.ledTags[i] = IO_TAG_NONE; + } +#ifdef LED0_A + config->statusLedConfig.ledTags[0] = IO_TAG(LED0_A); +#endif +#ifdef LED1_A + config->statusLedConfig.ledTags[1] = IO_TAG(LED1_A); +#endif +#ifdef LED2_A + config->statusLedConfig.ledTags[2] = IO_TAG(LED2_A); +#endif + } + config->rxConfig.spektrum_sat_bind = 5; config->rxConfig.spektrum_sat_bind_autoreset = 1; config->compassConfig.mag_hardware = MAG_NONE; // disabled by default diff --git a/src/main/target/ALIENFLIGHTF3/initialisation.c b/src/main/target/ALIENFLIGHTF3/initialisation.c new file mode 100644 index 000000000..53fbb1f91 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF3/initialisation.c @@ -0,0 +1,48 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" +#include "hardware_revision.h" + +void targetBusInit(void) +{ + #ifdef USE_SPI + #ifdef USE_SPI_DEVICE_1 + spiInit(SPIDEV_1); + #endif + #ifdef USE_SPI_DEVICE_2 + spiInit(SPIDEV_2); + #endif + #ifdef USE_SPI_DEVICE_3 + if (hardwareRevision == AFF3_REV_2) { + spiInit(SPIDEV_3); + } + #endif + #ifdef USE_SPI_DEVICE_4 + spiInit(SPIDEV_4); + #endif + #endif + + #ifdef USE_I2C + i2cInit(I2C_DEVICE); + #endif +} \ No newline at end of file diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 63a3a9bb0..1627043e9 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -19,6 +19,7 @@ #define TARGET_BOARD_IDENTIFIER "AFF3" // AlienFlight F3. #define TARGET_CONFIG +#define TARGET_BUS_INIT #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c index 7261aa223..cb70d05f7 100644 --- a/src/main/target/BLUEJAYF4/hardware_revision.c +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -85,7 +85,8 @@ void updateHardwareRevision(void) /* if flash exists on PB3 then Rev1 */ - if (m25p16_init(IO_TAG(PB3))) { + flashConfig_t flashConfig = { .csTag = IO_TAG(PB3) }; + if (m25p16_init(&flashConfig)) { hardwareRevision = BJF4_REV1; } else { IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, 0); diff --git a/src/main/target/CJMCU/initialisation.c b/src/main/target/CJMCU/initialisation.c new file mode 100644 index 000000000..817dfc540 --- /dev/null +++ b/src/main/target/CJMCU/initialisation.c @@ -0,0 +1,36 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" +#include "io/serial.h" + +void targetBusInit(void) +{ + #if defined(USE_SPI) && defined(USE_SPI_DEVICE_1) + spiInit(SPIDEV_1); + #endif + + if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { + serialRemovePort(SERIAL_PORT_USART3); + i2cInit(I2C_DEVICE); + } +} \ No newline at end of file diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index de5afbd44..4f3d274b9 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -19,6 +19,7 @@ #define TARGET_BOARD_IDENTIFIER "CJM1" // CJMCU #define USE_HARDWARE_REVISION_DETECTION +#define TARGET_BUS_INIT #define LED0 PC14 #define LED1 PC13 diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index b4142093e..6e8e07591 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -35,6 +35,8 @@ void targetConfiguration(master_t *config) { + UNUSED(config); + #ifdef BEEBRAIN // alternative defaults settings for Beebrain target config->motorConfig.motorPwmRate = 4000; @@ -88,9 +90,8 @@ void targetConfiguration(master_t *config) } else { config->beeperConfig.isOpenDrain = true; config->beeperConfig.isInverted = false; + config->flashConfig.csTag = IO_TAG_NONE; } -#else - UNUSED(config); #endif } diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index 533d8bb58..031b4d43a 100755 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -127,3 +127,4 @@ const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void) } #endif } + diff --git a/src/main/target/NAZE/initialisation.c b/src/main/target/NAZE/initialisation.c new file mode 100644 index 000000000..9ff56b1f6 --- /dev/null +++ b/src/main/target/NAZE/initialisation.c @@ -0,0 +1,44 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" +#include "drivers/bus_i2c.h" +#include "drivers/bus_spi.h" +#include "io/serial.h" +#include "hardware_revision.h" + +void targetBusInit(void) +{ + #ifdef USE_SPI + #ifdef USE_SPI_DEVICE_2 + spiInit(SPIDEV_2); + #endif + #endif + + if (hardwareRevision != NAZE32_SP) { + i2cInit(I2C_DEVICE); + serialRemovePort(SERIAL_PORT_SOFTSERIAL2); + } else { + if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { + serialRemovePort(SERIAL_PORT_USART3); + i2cInit(I2C_DEVICE); + } + } +} \ No newline at end of file diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 184ea76ee..2272a5552 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -19,6 +19,7 @@ #define TARGET_CONFIG #define USE_HARDWARE_REVISION_DETECTION +#define TARGET_BUS_INIT #define CLI_MINIMAL_VERBOSITY @@ -65,19 +66,14 @@ #define USE_SPI_DEVICE_2 #define NAZE_SPI_INSTANCE SPI2 -#define NAZE_SPI_CS_GPIO GPIOB #define NAZE_SPI_CS_PIN PB12 -#define NAZE_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB // We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO #define M25P16_CS_PIN NAZE_SPI_CS_PIN #define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE -#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL -#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO -#define MPU6500_CS_PIN NAZE_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE +#define MPU6500_CS_PIN NAZE_SPI_CS_PIN +#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 3d906295e..1c6ab3dd9 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -98,6 +98,7 @@ #define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 +#define SONAR_SOFTSERIAL2_EXCLUSIVE #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 79c1508ac..451404d34 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -88,6 +88,7 @@ #define SOFTSERIAL_1_TIMER TIM2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 +#define SONAR_SOFTSERIAL1_EXCLUSIVE #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA @@ -129,13 +130,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define TRANSPONDER diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 8ade29012..00c4a08c4 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -71,7 +71,7 @@ #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 - +#define SONAR_SOFTSERIAL1_EXCLUSIVE #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA From b91b7a304dab58f8685c470670e308746d6c227a Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 10 Dec 2016 10:37:48 +1100 Subject: [PATCH 2/3] Corrected bits for LEDs for AFF3 given no extended LEDs in list now --- src/main/target/ALIENFLIGHTF3/config.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 4bf94258b..8d208fb7d 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -49,13 +49,13 @@ void targetConfiguration(master_t *config) if (hardwareRevision == AFF3_REV_1) { config->statusLedConfig.polarity = 0 #ifdef LED0_A_INVERTED - | BIT(3) + | BIT(0) #endif #ifdef LED1_A_INVERTED - | BIT(4) + | BIT(1) #endif #ifdef LED2_A_INVERTED - | BIT(5) + | BIT(2) #endif ; From 1040168b9a1dc48ca19aa8563543cfd27deefcd6 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 10 Dec 2016 10:40:21 +1100 Subject: [PATCH 3/3] Alternative LED configuration is for AFF3 REV2 --- src/main/target/ALIENFLIGHTF3/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 8d208fb7d..449bdaf6d 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -46,7 +46,7 @@ void targetConfiguration(master_t *config) { /* depending on revision ... depends on the LEDs to be utilised. */ - if (hardwareRevision == AFF3_REV_1) { + if (hardwareRevision == AFF3_REV_2) { config->statusLedConfig.polarity = 0 #ifdef LED0_A_INVERTED | BIT(0)