From 5f435f7aadd3937429776297604a84fa7315b694 Mon Sep 17 00:00:00 2001 From: fishpepper Date: Mon, 7 Nov 2016 22:01:25 +0100 Subject: [PATCH 01/20] adding tinyfish fc --- src/main/target/TINYFISH/config.c | 56 +++++++++++++ src/main/target/TINYFISH/target.c | 35 ++++++++ src/main/target/TINYFISH/target.h | 123 +++++++++++++++++++++++++++++ src/main/target/TINYFISH/target.mk | 7 ++ 4 files changed, 221 insertions(+) create mode 100644 src/main/target/TINYFISH/config.c create mode 100644 src/main/target/TINYFISH/target.c create mode 100644 src/main/target/TINYFISH/target.h create mode 100644 src/main/target/TINYFISH/target.mk diff --git a/src/main/target/TINYFISH/config.c b/src/main/target/TINYFISH/config.c new file mode 100644 index 0000000000..2812a1d7a4 --- /dev/null +++ b/src/main/target/TINYFISH/config.c @@ -0,0 +1,56 @@ +/* + * 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 "drivers/io.h" + +#include "drivers/timer.h" +#include "drivers/dma.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/serial.h" + +#include "target.h" + +#define TARGET_CPU_VOLTAGE 3.0 + +// set default settings to match our target +void targetConfiguration(master_t *config) +{ + config->batteryConfig.currentMeterOffset = 0; + // we use an ina139, RL=0.005, Rs=30000 + // V/A = (0.005 * 0.001 * 30000) * I + // rescale to 1/10th mV / A -> * 1000 * 10 + // we use 3.0V as cpu and adc voltage -> rescale by 3.0/3.3 + config->batteryConfig.currentMeterScale = (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3); + + // we use the same uart for frsky telemetry and SBUS, both non inverted + int index = findSerialPortIndexByIdentifier(SBUS_TELEMETRY_UART); + config->serialConfig.portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; + + config->rxConfig.serialrx_provider = SERIALRX_SBUS; + config->telemetryConfig.telemetry_inversion = 0; + config->rxConfig.sbus_inversion = 0; + + intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures); +} + diff --git a/src/main/target/TINYFISH/target.c b/src/main/target/TINYFISH/target.c new file mode 100644 index 0000000000..8f16d10536 --- /dev/null +++ b/src/main/target/TINYFISH/target.c @@ -0,0 +1,35 @@ +/* + * 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 "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH5 + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA2_CH1 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH7 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH1 + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, TIMER_OUTPUT_ENABLED) //DMA1_CH2 - LED +}; + diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h new file mode 100644 index 0000000000..86b41936a9 --- /dev/null +++ b/src/main/target/TINYFISH/target.h @@ -0,0 +1,123 @@ +/* + * 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 . + */ + +#pragma once + +#define USB_VCP_ENABLED 1 + +#define TARGET_BOARD_IDENTIFIER "TFSH" // http://fishpepper.de/projects/tinyFISH + + +#define LED0 PC14 +#define LED1 PC15 + +#define BEEPER PB2 + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT + +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG_FLIP + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG_FLIP + + +#if USB_VCP_ENABLED + #define USE_VCP + #define USB_IO + #define USBD_PRODUCT_STRING "tinyFISH" + #define SERIAL_PORT_COUNT 4 +#else + #define SERIAL_PORT_COUNT 3 +#endif + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART2 + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI +#define USE_SPI_DEVICE_2 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define M25P16_CS_PIN SPI2_NSS_PIN +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PB1 +#define CURRENT_METER_ADC_PIN PB0 +#define ADC_INSTANCE ADC3 +#define VBAT_SCALE_DEFAULT 100 + +#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 USE_DSHOT + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_TELEMETRY) +#define TARGET_CONFIG + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(4) | TIM_N(8)) diff --git a/src/main/target/TINYFISH/target.mk b/src/main/target/TINYFISH/target.mk new file mode 100644 index 0000000000..ee1225563f --- /dev/null +++ b/src/main/target/TINYFISH/target.mk @@ -0,0 +1,7 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/flash_m25p16.c \ + drivers/accgyro_spi_mpu6000.c From cf74e556368113146add1a8e9489f7e075c84935 Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 1 Dec 2016 00:46:33 +1300 Subject: [PATCH 02/20] Fixed servo init for AUX forwarding. --- src/main/flight/servos.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 0469238cbb..707957aa54 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -177,7 +177,7 @@ void servoMixerInit(servoMixer_t *initialCustomServoMixers) // enable servos for mixes that require them. note, this shifts motor counts. useServo = mixers[currentMixerMode].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT)) + if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING)) useServo = 1; // give all servos a default command From ff6752cf38aa53350dfde4a03c80787c54084208 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 2 Dec 2016 19:57:03 +1300 Subject: [PATCH 03/20] Moved 'releaseSharedTelemetryPorts' to telemetry.c. --- src/main/fc/mw.c | 12 ------------ src/main/telemetry/telemetry.c | 11 +++++++++++ src/main/telemetry/telemetry.h | 2 ++ 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 82e2d5423e..e3e767df37 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -373,18 +373,6 @@ void mwDisarm(void) } } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT) - -#ifdef TELEMETRY -static void releaseSharedTelemetryPorts(void) { - serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - while (sharedPort) { - mspSerialReleasePortIfAllocated(sharedPort); - sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - } -} -#endif - void mwArm(void) { static bool firstArmingCalibrationWasCompleted; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 6b59e72d1d..2579187fc1 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -35,6 +35,8 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "msp/msp_serial.h" + #include "rx/rx.h" #include "telemetry/telemetry.h" @@ -156,4 +158,13 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb #endif } +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT) + +void releaseSharedTelemetryPorts(void) { + serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + while (sharedPort) { + mspSerialReleasePortIfAllocated(sharedPort); + sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + } +} #endif diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 9355c703db..f656578b95 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -60,3 +60,5 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing); void telemetryUseConfig(telemetryConfig_t *telemetryConfig); #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) + +void releaseSharedTelemetryPorts(void); From 8667500d8993d40a841777e2fd7d1d802cfa805c Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 3 Dec 2016 20:14:17 +1100 Subject: [PATCH 04/20] Added define in target.h for custom PLL_M (mhz for clock input) --- src/main/target/COLIBRI/target.h | 3 +-- src/main/target/system_stm32f4xx.c | 27 +++++++-------------------- 2 files changed, 8 insertions(+), 22 deletions(-) diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 702f04c740..b48f412fd1 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -26,8 +26,7 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define PLL_M 16 -#define PLL_N 336 +#define TARGET_XTAL_MHZ 16 #define LED0 PC14 #define LED1 PC13 diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 422db514c6..6ad22d1483 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -316,6 +316,7 @@ #include "stm32f4xx.h" #include "system_stm32f4xx.h" +#include "platform.h" uint32_t hse_value = HSE_VALUE; @@ -354,19 +355,19 @@ uint32_t hse_value = HSE_VALUE; /******************************************************************************/ /************************* PLL Parameters *************************************/ +#if defined(TARGET_XTAL_MHZ) + #define PLL_M TARGET_XTAL_MHZ +#else #if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ -#if defined(COLIBRI) - #define PLL_M 16 -#else #define PLL_M 8 -#endif #elif defined (STM32F446xx) #define PLL_M 8 #elif defined (STM32F410xx) || defined (STM32F411xE) #define PLL_M 8 #else #endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F469_479xx */ +#endif #if defined(STM32F446xx) /* PLL division factor for I2S, SAI, SYSTEM and SPDIF: Clock = PLL_VCO / PLLR */ @@ -422,22 +423,8 @@ uint32_t hse_value = HSE_VALUE; /** @addtogroup STM32F4xx_System_Private_Variables * @{ */ - -#if defined(STM32F40_41xxx) - uint32_t SystemCoreClock = 168000000; -#endif /* STM32F40_41xxx */ - -#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx) - uint32_t SystemCoreClock = 180000000; -#endif /* STM32F427_437x || STM32F429_439xx || STM32F446xx || STM32F469_479xx */ - -#if defined(STM32F401xx) - uint32_t SystemCoreClock = 84000000; -#endif /* STM32F401xx */ - -#if defined(STM32F410xx) || defined(STM32F411xE) - uint32_t SystemCoreClock = 96000000; -#endif /* STM32F410xx || STM32F401xE */ +/* core clock is simply a mhz of PLL_N / PLL_P */ +uint32_t SystemCoreClock = (PLL_N / PLL_P) * 1000000; __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; From 38956119f701db532cefca7a7a4ed0df1aca54b9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 3 Dec 2016 20:19:23 +1100 Subject: [PATCH 05/20] removed unnecessary comment --- src/main/target/system_stm32f4xx.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 6ad22d1483..e7e6d04063 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -359,7 +359,6 @@ uint32_t hse_value = HSE_VALUE; #define PLL_M TARGET_XTAL_MHZ #else #if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) - /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ #define PLL_M 8 #elif defined (STM32F446xx) #define PLL_M 8 From 46387c9dbecc338d2391d2cbee4df1c3bd78aad7 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 3 Dec 2016 15:02:01 +0100 Subject: [PATCH 06/20] Cleanup ENUM usage --- src/main/blackbox/blackbox_io.h | 1 - src/main/common/axis.h | 2 +- src/main/config/parameter_group.h | 4 ++-- src/main/drivers/adc.h | 4 +--- src/main/drivers/adc_impl.h | 11 ++--------- src/main/drivers/bus_i2c.h | 2 +- src/main/drivers/bus_i2c_hal.c | 2 +- src/main/drivers/bus_spi.h | 9 ++++----- src/main/drivers/bus_spi_soft.h | 3 +-- src/main/drivers/pwm_rx.c | 2 +- src/main/drivers/rcc.h | 2 +- src/main/drivers/rx_nrf24l01.h | 4 ++-- src/main/drivers/rx_spi.h | 2 +- src/main/drivers/sdcard.c | 4 ++-- src/main/drivers/sdcard.h | 2 +- src/main/drivers/serial_escserial.c | 4 ++-- src/main/drivers/vcd.h | 6 +++++- src/main/fc/config.h | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/fc/rc_controls.h | 3 +-- src/main/fc/runtime_config.h | 4 ++-- src/main/flight/servos.h | 4 +--- src/main/io/asyncfatfs/asyncfatfs.c | 18 +++++++++--------- src/main/io/asyncfatfs/asyncfatfs.h | 6 +++--- src/main/io/asyncfatfs/fat_standard.h | 2 +- src/main/io/beeper.h | 2 +- src/main/io/flashfs.c | 2 +- src/main/io/gps.c | 2 +- src/main/io/gps.h | 6 ++---- src/main/io/ledstrip.c | 2 +- src/main/io/ledstrip.h | 4 ++-- src/main/io/serial.h | 5 ++--- src/main/io/serial_cli.c | 2 +- src/main/rx/nrf24_cx10.c | 2 +- src/main/rx/nrf24_inav.c | 4 ++-- src/main/rx/nrf24_syma.c | 2 +- src/main/rx/rx.h | 8 +++----- src/main/sensors/acceleration.h | 3 +-- src/main/sensors/barometer.h | 3 +-- src/main/sensors/battery.h | 6 ++---- src/main/sensors/compass.h | 3 +-- src/main/sensors/gyro.h | 3 +-- src/main/sensors/sensors.h | 2 +- src/main/target/COLIBRI_RACE/bus_bst.h | 3 +-- src/main/telemetry/crsf.c | 6 +++--- src/main/telemetry/crsf.h | 3 +-- src/main/telemetry/esc_telemetry.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/hott.h | 4 ++-- src/main/telemetry/smartport.c | 6 +++--- src/test/unit/platform.h | 2 +- 51 files changed, 85 insertions(+), 109 deletions(-) diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 5cb6548087..8a8087f180 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -27,7 +27,6 @@ typedef enum BlackboxDevice { BLACKBOX_DEVICE_SDCARD = 2, #endif - BLACKBOX_DEVICE_END } BlackboxDevice; typedef enum { diff --git a/src/main/common/axis.h b/src/main/common/axis.h index c7f1a96a87..387d96c265 100644 --- a/src/main/common/axis.h +++ b/src/main/common/axis.h @@ -36,7 +36,7 @@ typedef enum { typedef enum { AI_ROLL = 0, - AI_PITCH, + AI_PITCH } angle_index_t; #define ANGLE_INDEX_COUNT 2 diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index c5133a2244..e7059aaf9d 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -22,7 +22,7 @@ typedef uint16_t pgn_t; // parameter group registry flags typedef enum { PGRF_NONE = 0, - PGRF_CLASSIFICATON_BIT = (1 << 0), + PGRF_CLASSIFICATON_BIT = (1 << 0) } pgRegistryFlags_e; typedef enum { @@ -30,7 +30,7 @@ typedef enum { PGR_PGN_VERSION_MASK = 0xf000, PGR_SIZE_MASK = 0x0fff, PGR_SIZE_SYSTEM_FLAG = 0x0000, // documentary - PGR_SIZE_PROFILE_FLAG = 0x8000, // start using flags from the top bit down + PGR_SIZE_PROFILE_FLAG = 0x8000 // start using flags from the top bit down } pgRegistryInternal_e; // function that resets a single parameter group instance diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 5ac3f02e06..50c6f925a8 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -24,11 +24,9 @@ typedef enum { ADC_CURRENT = 1, ADC_EXTERNAL1 = 2, ADC_RSSI = 3, - ADC_CHANNEL_MAX = ADC_RSSI + ADC_CHANNEL_COUNT } AdcChannel; -#define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1) - typedef struct adc_config_s { ioTag_t tag; uint8_t adcChannel; // ADC1_INxx channel number diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 3c29f70da5..51f562e94d 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -31,16 +31,9 @@ typedef enum ADCDevice { ADCINVALID = -1, ADCDEV_1 = 0, -#if defined(STM32F3) +#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) ADCDEV_2, - ADCDEV_3, - ADCDEV_MAX = ADCDEV_3, -#elif defined(STM32F4) || defined(STM32F7) - ADCDEV_2, - ADCDEV_3, - ADCDEV_MAX = ADCDEV_3, -#else - ADCDEV_MAX = ADCDEV_1, + ADCDEV_3 #endif } ADCDevice; diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index da2da2ff7c..746933bd80 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -34,7 +34,7 @@ typedef enum I2CDevice { I2CDEV_2, I2CDEV_3, I2CDEV_4, - I2CDEV_MAX = I2CDEV_4, + I2CDEV_COUNT } I2CDevice; typedef struct i2cDevice_s { diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 8d8f98408e..040e8a6037 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -80,7 +80,7 @@ static i2cDevice_t i2cHardwareMap[] = { typedef struct{ I2C_HandleTypeDef Handle; }i2cHandle_t; -static i2cHandle_t i2cHandle[I2CDEV_MAX+1]; +static i2cHandle_t i2cHandle[I2CDEV_COUNT]; void I2C1_ER_IRQHandler(void) { diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 9692fb92a3..2b93923d2c 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -46,17 +46,17 @@ typedef enum { SPI_CLOCK_SLOW = 128, //00.65625 MHz SPI_CLOCK_STANDARD = 8, //10.50000 MHz SPI_CLOCK_FAST = 4, //21.00000 MHz - SPI_CLOCK_ULTRAFAST = 2, //42.00000 MHz + SPI_CLOCK_ULTRAFAST = 2 //42.00000 MHz #elif defined(STM32F7) SPI_CLOCK_SLOW = 256, //00.42188 MHz SPI_CLOCK_STANDARD = 16, //06.57500 MHz SPI_CLOCK_FAST = 4, //27.00000 MHz - SPI_CLOCK_ULTRAFAST = 2, //54.00000 MHz + SPI_CLOCK_ULTRAFAST = 2 //54.00000 MHz #else SPI_CLOCK_SLOW = 128, //00.56250 MHz SPI_CLOCK_STANDARD = 4, //09.00000 MHz SPI_CLOCK_FAST = 2, //18.00000 MHz - SPI_CLOCK_ULTRAFAST = 2, //18.00000 MHz + SPI_CLOCK_ULTRAFAST = 2 //18.00000 MHz #endif } SPIClockDivider_e; @@ -65,8 +65,7 @@ typedef enum SPIDevice { SPIDEV_1 = 0, SPIDEV_2, SPIDEV_3, - SPIDEV_4, - SPIDEV_MAX = SPIDEV_4, + SPIDEV_4 } SPIDevice; typedef struct SPIDevice_s { diff --git a/src/main/drivers/bus_spi_soft.h b/src/main/drivers/bus_spi_soft.h index 88b106e3f4..facb0be93b 100644 --- a/src/main/drivers/bus_spi_soft.h +++ b/src/main/drivers/bus_spi_soft.h @@ -20,8 +20,7 @@ #include "io_types.h" typedef enum softSPIDevice { - SOFT_SPIDEV_1 = 0, - SOFT_SPIDEV_MAX = SOFT_SPIDEV_1, + SOFT_SPIDEV_1 = 0 } softSPIDevice_e; typedef struct softSPIDevice_s { diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index cc8aad33fa..2adaa8af6c 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -55,7 +55,7 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); typedef enum { INPUT_MODE_PPM, - INPUT_MODE_PWM, + INPUT_MODE_PWM } pwmInputMode_t; typedef struct { diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index 4dee74ee79..6c1384ff3d 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -7,7 +7,7 @@ enum rcc_reg { RCC_AHB, RCC_APB2, RCC_APB1, - RCC_AHB1, + RCC_AHB1 }; #define RCC_ENCODE(reg, mask) (((reg) << 5) | LOG2_32BIT(mask)) diff --git a/src/main/drivers/rx_nrf24l01.h b/src/main/drivers/rx_nrf24l01.h index 08d2c8afa7..27d3c65cb7 100644 --- a/src/main/drivers/rx_nrf24l01.h +++ b/src/main/drivers/rx_nrf24l01.h @@ -106,7 +106,7 @@ enum { NRF24L01_1D_FEATURE_EN_DPL = 2, NRF24L01_1D_FEATURE_EN_ACK_PAY = 1, - NRF24L01_1D_FEATURE_EN_DYN_ACK = 0, + NRF24L01_1D_FEATURE_EN_DYN_ACK = 0 }; // Pre-shifted and combined bits @@ -162,7 +162,7 @@ enum { NRF24L01_06_RF_SETUP_RF_PWR_n6dbm = 0x04, NRF24L01_06_RF_SETUP_RF_PWR_0dbm = 0x06, - NRF24L01_1C_DYNPD_ALL_PIPES = 0x3F, + NRF24L01_1C_DYNPD_ALL_PIPES = 0x3F }; // Pipes diff --git a/src/main/drivers/rx_spi.h b/src/main/drivers/rx_spi.h index f083ca5058..0afd720fcb 100644 --- a/src/main/drivers/rx_spi.h +++ b/src/main/drivers/rx_spi.h @@ -20,7 +20,7 @@ typedef enum { RX_SPI_SOFTSPI, - RX_SPI_HARDSPI, + RX_SPI_HARDSPI } rx_spi_type_e; #define RX_SPI_MAX_PAYLOAD_SIZE 32 diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index e74303f179..66aafc25b2 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -67,7 +67,7 @@ typedef enum { SDCARD_STATE_SENDING_WRITE, SDCARD_STATE_WAITING_FOR_WRITE, SDCARD_STATE_WRITING_MULTIPLE_BLOCKS, - SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE, + SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE } sdcardState_e; typedef struct sdcard_t { @@ -352,7 +352,7 @@ static bool sdcard_readOCRRegister(uint32_t *result) typedef enum { SDCARD_RECEIVE_SUCCESS, SDCARD_RECEIVE_BLOCK_IN_PROGRESS, - SDCARD_RECEIVE_ERROR, + SDCARD_RECEIVE_ERROR } sdcardReceiveBlockStatus_e; /** diff --git a/src/main/drivers/sdcard.h b/src/main/drivers/sdcard.h index b6b7ae0b2f..aad46a0dce 100644 --- a/src/main/drivers/sdcard.h +++ b/src/main/drivers/sdcard.h @@ -43,7 +43,7 @@ typedef struct sdcardMetadata_s { typedef enum { SDCARD_BLOCK_OPERATION_READ, SDCARD_BLOCK_OPERATION_WRITE, - SDCARD_BLOCK_OPERATION_ERASE, + SDCARD_BLOCK_OPERATION_ERASE } sdcardBlockOperation_e; typedef enum { diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 6b91bae063..351b2c98cc 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -24,7 +24,7 @@ typedef enum { BAUDRATE_NORMAL = 19200, BAUDRATE_KISS = 38400, - BAUDRATE_CASTLE = 18880, + BAUDRATE_CASTLE = 18880 } escBaudRate_e; typedef enum { @@ -32,7 +32,7 @@ typedef enum { PROTOCOL_BLHELI = 1, PROTOCOL_KISS = 2, PROTOCOL_KISSALL = 3, - PROTOCOL_CASTLE = 4, + PROTOCOL_CASTLE = 4 } escProtocol_e; #if defined(USE_ESCSERIAL) diff --git a/src/main/drivers/vcd.h b/src/main/drivers/vcd.h index 98aad231f5..1d279a84f4 100644 --- a/src/main/drivers/vcd.h +++ b/src/main/drivers/vcd.h @@ -25,4 +25,8 @@ typedef struct vcdProfile_s { int8_t v_offset; } vcdProfile_t; -enum VIDEO_SYSTEMS { VIDEO_SYSTEM_AUTO = 0, VIDEO_SYSTEM_PAL, VIDEO_SYSTEM_NTSC }; +enum VIDEO_SYSTEMS { + VIDEO_SYSTEM_AUTO = 0, + VIDEO_SYSTEM_PAL, + VIDEO_SYSTEM_NTSC +}; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 3a3c4334a6..487cad9dc4 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,7 +55,7 @@ typedef enum { FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, - FEATURE_ESC_TELEMETRY = 1 << 27, + FEATURE_ESC_TELEMETRY = 1 << 27 } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6e2fa44ea9..bda80ccc8f 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -176,7 +176,7 @@ typedef enum { } mspSDCardState_e; typedef enum { - MSP_SDCARD_FLAG_SUPPORTTED = 1, + MSP_SDCARD_FLAG_SUPPORTTED = 1 } mspSDCardFlags_e; #define RATEPROFILE_MASK (1 << 7) diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 7d50636d3b..13c5dc6cb0 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -207,8 +207,7 @@ typedef enum { ADJUSTMENT_RC_RATE_YAW, ADJUSTMENT_D_SETPOINT, ADJUSTMENT_D_SETPOINT_TRANSITION, - ADJUSTMENT_FUNCTION_COUNT, - + ADJUSTMENT_FUNCTION_COUNT } adjustmentFunction_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 8366f96067..04441c565e 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -43,7 +43,7 @@ typedef enum { PASSTHRU_MODE = (1 << 8), SONAR_MODE = (1 << 9), FAILSAFE_MODE = (1 << 10), - GTUNE_MODE = (1 << 11), + GTUNE_MODE = (1 << 11) } flightModeFlags_e; extern uint16_t flightModeFlags; @@ -57,7 +57,7 @@ typedef enum { GPS_FIX = (1 << 1), CALIBRATE_MAG = (1 << 2), SMALL_ANGLE = (1 << 3), - FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code + FIXED_WING = (1 << 4) // set when in flying_wing or airplane mode. currently used by althold selection code } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 66f86955b6..756a0667ed 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -35,7 +35,6 @@ enum { INPUT_RC_AUX4, INPUT_GIMBAL_PITCH, INPUT_GIMBAL_ROLL, - INPUT_SOURCE_COUNT } inputSource_e; @@ -59,8 +58,7 @@ typedef enum { SERVO_SINGLECOPTER_1 = 3, SERVO_SINGLECOPTER_2 = 4, SERVO_SINGLECOPTER_3 = 5, - SERVO_SINGLECOPTER_4 = 6, - + SERVO_SINGLECOPTER_4 = 6 } servoIndex_e; // FIXME rename to servoChannel_e #define SERVO_PLANE_INDEX_MIN SERVO_FLAPS diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 57b8f0c861..040a2e5848 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -98,7 +98,7 @@ typedef enum { AFATFS_SAVE_DIRECTORY_NORMAL, AFATFS_SAVE_DIRECTORY_FOR_CLOSE, - AFATFS_SAVE_DIRECTORY_DELETED, + AFATFS_SAVE_DIRECTORY_DELETED } afatfsSaveDirectoryEntryMode_e; typedef enum { @@ -119,7 +119,7 @@ typedef enum { typedef enum { CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR, CLUSTER_SEARCH_FREE, - CLUSTER_SEARCH_OCCUPIED, + CLUSTER_SEARCH_OCCUPIED } afatfsClusterSearchCondition_e; enum { @@ -127,14 +127,14 @@ enum { AFATFS_CREATEFILE_PHASE_FIND_FILE, AFATFS_CREATEFILE_PHASE_CREATE_NEW_FILE, AFATFS_CREATEFILE_PHASE_SUCCESS, - AFATFS_CREATEFILE_PHASE_FAILURE, + AFATFS_CREATEFILE_PHASE_FAILURE }; typedef enum { AFATFS_FIND_CLUSTER_IN_PROGRESS, AFATFS_FIND_CLUSTER_FOUND, AFATFS_FIND_CLUSTER_FATAL, - AFATFS_FIND_CLUSTER_NOT_FOUND, + AFATFS_FIND_CLUSTER_NOT_FOUND } afatfsFindClusterStatus_e; struct afatfsFileOperation_t; @@ -234,7 +234,7 @@ typedef enum { AFATFS_APPEND_SUPERCLUSTER_PHASE_INIT = 0, AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FREEFILE_DIRECTORY, AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FAT, - AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY, + AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY } afatfsAppendSuperclusterPhase_e; typedef struct afatfsAppendSupercluster_t { @@ -251,7 +251,7 @@ typedef enum { AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT2, AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY, AFATFS_APPEND_FREE_CLUSTER_PHASE_COMPLETE, - AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE, + AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE } afatfsAppendFreeClusterPhase_e; typedef struct afatfsAppendFreeCluster_t { @@ -286,7 +286,7 @@ typedef enum { AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_CONTIGUOUS, AFATFS_TRUNCATE_FILE_PREPEND_TO_FREEFILE, #endif - AFATFS_TRUNCATE_FILE_SUCCESS, + AFATFS_TRUNCATE_FILE_SUCCESS } afatfsTruncateFilePhase_e; typedef struct afatfsTruncateFile_t { @@ -299,7 +299,7 @@ typedef struct afatfsTruncateFile_t { typedef enum { AFATFS_DELETE_FILE_DELETE_DIRECTORY_ENTRY, - AFATFS_DELETE_FILE_DEALLOCATE_CLUSTERS, + AFATFS_DELETE_FILE_DEALLOCATE_CLUSTERS } afatfsDeleteFilePhase_e; typedef struct afatfsDeleteFile_t { @@ -323,7 +323,7 @@ typedef enum { AFATFS_FILE_OPERATION_LOCKED, #endif AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER, - AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY, + AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY } afatfsFileOperation_e; typedef struct afatfsFileOperation_t { diff --git a/src/main/io/asyncfatfs/asyncfatfs.h b/src/main/io/asyncfatfs/asyncfatfs.h index fd515694bb..faeb7ea510 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.h +++ b/src/main/io/asyncfatfs/asyncfatfs.h @@ -28,13 +28,13 @@ typedef enum { AFATFS_FILESYSTEM_STATE_UNKNOWN, AFATFS_FILESYSTEM_STATE_FATAL, AFATFS_FILESYSTEM_STATE_INITIALIZATION, - AFATFS_FILESYSTEM_STATE_READY, + AFATFS_FILESYSTEM_STATE_READY } afatfsFilesystemState_e; typedef enum { AFATFS_OPERATION_IN_PROGRESS, AFATFS_OPERATION_SUCCESS, - AFATFS_OPERATION_FAILURE, + AFATFS_OPERATION_FAILURE } afatfsOperationStatus_e; typedef enum { @@ -54,7 +54,7 @@ typedef afatfsDirEntryPointer_t afatfsFinder_t; typedef enum { AFATFS_SEEK_SET, AFATFS_SEEK_CUR, - AFATFS_SEEK_END, + AFATFS_SEEK_END } afatfsSeek_e; typedef void (*afatfsFileCallback_t)(afatfsFilePtr_t file); diff --git a/src/main/io/asyncfatfs/fat_standard.h b/src/main/io/asyncfatfs/fat_standard.h index 590bc43a55..4d63b2c65b 100644 --- a/src/main/io/asyncfatfs/fat_standard.h +++ b/src/main/io/asyncfatfs/fat_standard.h @@ -54,7 +54,7 @@ typedef enum { FAT_FILESYSTEM_TYPE_INVALID, FAT_FILESYSTEM_TYPE_FAT12, FAT_FILESYSTEM_TYPE_FAT16, - FAT_FILESYSTEM_TYPE_FAT32, + FAT_FILESYSTEM_TYPE_FAT32 } fatFilesystemType_e; typedef struct mbrPartitionEntry_t { diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 0822b285bb..15a40eef5d 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -43,7 +43,7 @@ typedef enum { BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_ALL, // Turn ON or OFF all beeper conditions - BEEPER_PREFERENCE, // Save preferred beeper configuration + BEEPER_PREFERENCE // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum } beeperMode_e; diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c index de897b7c63..5e346d1b82 100644 --- a/src/main/io/flashfs.c +++ b/src/main/io/flashfs.c @@ -503,7 +503,7 @@ int flashfsIdentifyStartOfFreeSpace() /* We don't expect valid data to ever contain this many consecutive uint32_t's of all 1 bits: */ FREE_BLOCK_TEST_SIZE_INTS = 4, // i.e. 16 bytes - FREE_BLOCK_TEST_SIZE_BYTES = FREE_BLOCK_TEST_SIZE_INTS * sizeof(uint32_t), + FREE_BLOCK_TEST_SIZE_BYTES = FREE_BLOCK_TEST_SIZE_INTS * sizeof(uint32_t) }; union { diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 15a6767beb..c4925d8f4e 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -175,7 +175,7 @@ typedef enum { GPS_CHANGE_BAUD, GPS_CONFIGURE, GPS_RECEIVING_DATA, - GPS_LOST_COMMUNICATION, + GPS_LOST_COMMUNICATION } gpsState_e; gpsData_t gpsData; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 5e258f3a06..6cbced3e84 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -51,7 +51,7 @@ typedef enum { typedef enum { GPS_AUTOCONFIG_OFF = 0, - GPS_AUTOCONFIG_ON, + GPS_AUTOCONFIG_ON } gpsAutoConfig_e; typedef enum { @@ -78,11 +78,9 @@ typedef enum { GPS_MESSAGE_STATE_IDLE = 0, GPS_MESSAGE_STATE_INIT, GPS_MESSAGE_STATE_SBAS, - GPS_MESSAGE_STATE_MAX = GPS_MESSAGE_STATE_SBAS + GPS_MESSAGE_STATE_ENTRY_COUNT } gpsMessageState_e; -#define GPS_MESSAGE_STATE_ENTRY_COUNT (GPS_MESSAGE_STATE_MAX + 1) - typedef struct gpsData_s { uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices uint8_t baudrateIndex; // index into auto-detecting or current baudrate diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 310986ae2a..7fa0ce9199 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -114,7 +114,7 @@ typedef enum { COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_MAGENTA, - COLOR_DEEP_PINK, + COLOR_DEEP_PINK } colorId_e; const hsvColor_t hsv[] = { diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index b545a3a4ec..246cb9925e 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -108,7 +108,7 @@ typedef enum { LED_FUNCTION_BATTERY, LED_FUNCTION_RSSI, LED_FUNCTION_GPS, - LED_FUNCTION_THRUST_RING, + LED_FUNCTION_THRUST_RING } ledBaseFunctionId_e; typedef enum { @@ -117,7 +117,7 @@ typedef enum { LED_OVERLAY_BLINK, LED_OVERLAY_LANDING_FLASH, LED_OVERLAY_INDICATOR, - LED_OVERLAY_WARNING, + LED_OVERLAY_WARNING } ledOverlayId_e; typedef struct modeColorIndexes_s { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index ca2f22436c..c8d7e31785 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -37,7 +37,7 @@ typedef enum { FUNCTION_BLACKBOX = (1 << 7), // 128 FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 - FUNCTION_TELEMETRY_ESC = (1 << 10), // 1024 + FUNCTION_TELEMETRY_ESC = (1 << 10) // 1024 } serialPortFunction_e; typedef enum { @@ -74,8 +74,7 @@ typedef enum { SERIAL_PORT_USART8, SERIAL_PORT_USB_VCP = 20, SERIAL_PORT_SOFTSERIAL1 = 30, - SERIAL_PORT_SOFTSERIAL2, - SERIAL_PORT_IDENTIFIER_MAX = SERIAL_PORT_SOFTSERIAL2 + SERIAL_PORT_SOFTSERIAL2 } serialPortIdentifier_e; extern const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3cad9dd6d1..f7a9bdd923 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -210,7 +210,7 @@ typedef enum { DUMP_ALL = (1 << 3), DO_DIFF = (1 << 4), SHOW_DEFAULTS = (1 << 5), - HIDE_UNUSED = (1 << 6), + HIDE_UNUSED = (1 << 6) } dumpFlags_e; static const char* const emptyName = "-"; diff --git a/src/main/rx/nrf24_cx10.c b/src/main/rx/nrf24_cx10.c index 026eeba0c4..d7521e5687 100644 --- a/src/main/rx/nrf24_cx10.c +++ b/src/main/rx/nrf24_cx10.c @@ -61,7 +61,7 @@ enum { RATE_LOW = 0, RATE_MID = 1, - RATE_HIGH= 2, + RATE_HIGH= 2 }; #define FLAG_FLIP 0x10 // goes to rudder channel diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c index 1e8edd9814..115fc10d10 100644 --- a/src/main/rx/nrf24_inav.c +++ b/src/main/rx/nrf24_inav.c @@ -89,7 +89,7 @@ enum { RATE_LOW = 0, RATE_MID = 1, - RATE_HIGH = 2, + RATE_HIGH = 2 }; enum { @@ -97,7 +97,7 @@ enum { FLAG_PICTURE = 0x02, FLAG_VIDEO = 0x04, FLAG_RTH = 0x08, - FLAG_HEADLESS = 0x10, + FLAG_HEADLESS = 0x10 }; typedef enum { diff --git a/src/main/rx/nrf24_syma.c b/src/main/rx/nrf24_syma.c index afc2380726..61a51199b6 100644 --- a/src/main/rx/nrf24_syma.c +++ b/src/main/rx/nrf24_syma.c @@ -73,7 +73,7 @@ enum { RATE_LOW = 0, RATE_MID = 1, - RATE_HIGH= 2, + RATE_HIGH= 2 }; #define FLAG_PICTURE 0x40 diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7b881fd14e..1a5108638d 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -55,9 +55,7 @@ typedef enum { SERIALRX_XBUS_MODE_B_RJ01 = 6, SERIALRX_IBUS = 7, SERIALRX_JETIEXBUS = 8, - SERIALRX_CRSF = 9, - SERIALRX_PROVIDER_COUNT, - SERIALRX_PROVIDER_MAX = SERIALRX_PROVIDER_COUNT - 1 + SERIALRX_CRSF = 9 } SerialRXType; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12 @@ -89,14 +87,14 @@ typedef enum { RX_FAILSAFE_MODE_AUTO = 0, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET, - RX_FAILSAFE_MODE_INVALID, + RX_FAILSAFE_MODE_INVALID } rxFailsafeChannelMode_e; #define RX_FAILSAFE_MODE_COUNT 3 typedef enum { RX_FAILSAFE_TYPE_FLIGHT = 0, - RX_FAILSAFE_TYPE_AUX, + RX_FAILSAFE_TYPE_AUX } rxFailsafeChannelType_e; #define RX_FAILSAFE_TYPE_COUNT 2 diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 07be4da77a..811767a6b6 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -32,8 +32,7 @@ typedef enum { ACC_MPU6000 = 7, ACC_MPU6500 = 8, ACC_ICM20689 = 9, - ACC_FAKE = 10, - ACC_MAX = ACC_FAKE + ACC_FAKE = 10 } accelerationSensor_e; typedef struct acc_s { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index f684652519..fdd47e5f99 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -24,8 +24,7 @@ typedef enum { BARO_NONE = 1, BARO_BMP085 = 2, BARO_MS5611 = 3, - BARO_BMP280 = 4, - BARO_MAX = BARO_BMP280 + BARO_BMP280 = 4 } baroSensor_e; #define BARO_SAMPLE_COUNT_MAX 48 diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index e7821a475d..01309a8c8c 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -31,15 +31,13 @@ typedef enum { CURRENT_SENSOR_NONE = 0, CURRENT_SENSOR_ADC, CURRENT_SENSOR_VIRTUAL, - CURRENT_SENSOR_ESC, - CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC + CURRENT_SENSOR_ESC } currentSensor_e; typedef enum { BATTERY_SENSOR_NONE = 0, BATTERY_SENSOR_ADC, - BATTERY_SENSOR_ESC, - BATTERY_SENSOR_MAX = BATTERY_SENSOR_ESC + BATTERY_SENSOR_ESC } batterySensor_e; typedef struct batteryConfig_s { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index f64c517c29..a913174c55 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -28,8 +28,7 @@ typedef enum { MAG_NONE = 1, MAG_HMC5883 = 2, MAG_AK8975 = 3, - MAG_AK8963 = 4, - MAG_MAX = MAG_AK8963 + MAG_AK8963 = 4 } magSensor_e; typedef struct mag_s { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 3034dd5a09..8e9d988493 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,8 +31,7 @@ typedef enum { GYRO_MPU6500, GYRO_MPU9250, GYRO_ICM20689, - GYRO_FAKE, - GYRO_MAX = GYRO_FAKE + GYRO_FAKE } gyroSensor_e; typedef struct gyro_s { diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 3507a2965b..107a74c75d 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -49,7 +49,7 @@ typedef enum { SENSOR_MAG = 1 << 3, SENSOR_SONAR = 1 << 4, SENSOR_GPS = 1 << 5, - SENSOR_GPSMAG = 1 << 6, + SENSOR_GPSMAG = 1 << 6 } sensors_e; typedef enum { diff --git a/src/main/target/COLIBRI_RACE/bus_bst.h b/src/main/target/COLIBRI_RACE/bus_bst.h index 26eeb967d5..a610784123 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst.h +++ b/src/main/target/COLIBRI_RACE/bus_bst.h @@ -36,8 +36,7 @@ typedef enum BSTDevice { BSTDEV_1, - BSTDEV_2, - BSTDEV_MAX = BSTDEV_2, + BSTDEV_2 } BSTDevice; void bstInit(BSTDevice index); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 2361e05fd8..138487d147 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -196,13 +196,13 @@ void crsfFrameBatterySensor(sbuf_t *dst) typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, - CRSF_ACTIVE_ANTENNA2 = 1, + CRSF_ACTIVE_ANTENNA2 = 1 } crsfActiveAntenna_e; typedef enum { CRSF_RF_MODE_4_HZ = 0, CRSF_RF_MODE_50_HZ = 1, - CRSF_RF_MODE_150_HZ = 2, + CRSF_RF_MODE_150_HZ = 2 } crsrRfMode_e; typedef enum { @@ -212,7 +212,7 @@ typedef enum { CRSF_RF_POWER_100_mW = 3, CRSF_RF_POWER_500_mW = 4, CRSF_RF_POWER_1000_mW = 5, - CRSF_RF_POWER_2000_mW = 6, + CRSF_RF_POWER_2000_mW = 6 } crsrRfPower_e; /* diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 3685f512cc..cbdb437f82 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -26,8 +26,7 @@ typedef enum { CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, CRSF_FRAME_BATTERY_SENSOR, CRSF_FRAME_FLIGHT_MODE, - CRSF_FRAME_GPS, - CRSF_FRAME_COUNT + CRSF_FRAME_GPS } crsfFrameType_e; void initCrsfTelemetry(void); diff --git a/src/main/telemetry/esc_telemetry.c b/src/main/telemetry/esc_telemetry.c index 7404254eda..0bde741abd 100644 --- a/src/main/telemetry/esc_telemetry.c +++ b/src/main/telemetry/esc_telemetry.c @@ -75,7 +75,7 @@ typedef enum { typedef enum { ESC_TLM_TRIGGER_WAIT = 0, ESC_TLM_TRIGGER_READY = 1 << 0, // 1 - ESC_TLM_TRIGGER_PENDING = 1 << 1, // 2 + ESC_TLM_TRIGGER_PENDING = 1 << 1 // 2 } escTlmTriggerState_t; #define ESC_TLM_BAUDRATE 115200 diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 588b35dc47..c918edae6a 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -127,7 +127,7 @@ typedef enum { GPS_FIX_CHAR_NONE = '-', GPS_FIX_CHAR_2D = '2', GPS_FIX_CHAR_3D = '3', - GPS_FIX_CHAR_DGPS = 'D', + GPS_FIX_CHAR_DGPS = 'D' } gpsFixChar_e; static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size) diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index d01b45b34f..5370dc24f3 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -55,7 +55,7 @@ typedef enum { HOTT_EAM_ALARM1_FLAG_TEMPERATURE_2 = (1 << 4), HOTT_EAM_ALARM1_FLAG_ALTITUDE = (1 << 5), HOTT_EAM_ALARM1_FLAG_CURRENT = (1 << 6), - HOTT_EAM_ALARM1_FLAG_MAIN_VOLTAGE = (1 << 7), + HOTT_EAM_ALARM1_FLAG_MAIN_VOLTAGE = (1 << 7) } hottEamAlarm1Flag_e; typedef enum { @@ -67,7 +67,7 @@ typedef enum { HOTT_EAM_ALARM2_FLAG_M3S_DUPLICATE = (1 << 4), HOTT_EAM_ALARM2_FLAG_UNKNOWN_1 = (1 << 5), HOTT_EAM_ALARM2_FLAG_UNKNOWN_2 = (1 << 6), - HOTT_EAM_ALARM2_FLAG_ON_SIGN_OR_TEXT_ACTIVE = (1 << 7), + HOTT_EAM_ALARM2_FLAG_ON_SIGN_OR_TEXT_ACTIVE = (1 << 7) } hottEamAlarm2Flag_e; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 93f7ef4587..496013f143 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -65,7 +65,7 @@ enum SPSTATE_UNINITIALIZED, SPSTATE_INITIALIZED, SPSTATE_WORKING, - SPSTATE_TIMEDOUT, + SPSTATE_TIMEDOUT }; enum @@ -83,7 +83,7 @@ enum FSSP_SENSOR_ID1 = 0x1B, FSSP_SENSOR_ID2 = 0x0D, FSSP_SENSOR_ID3 = 0x34, - FSSP_SENSOR_ID4 = 0x67, + FSSP_SENSOR_ID4 = 0x67 // there are 32 ID's polled by smartport master // remaining 3 bits are crc (according to comments in openTx code) }; @@ -112,7 +112,7 @@ enum FSSP_DATAID_T2 = 0x0410 , FSSP_DATAID_GPS_ALT = 0x0820 , FSSP_DATAID_A3 = 0x0900 , - FSSP_DATAID_A4 = 0x0910 , + FSSP_DATAID_A4 = 0x0910 }; const uint16_t frSkyDataIdTable[] = { diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 4cec0f990a..c5c400aa85 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -35,7 +35,7 @@ typedef enum { Mode_TEST = 0x0, - Mode_Out_PP = 0x10, + Mode_Out_PP = 0x10 } GPIO_Mode; typedef enum {RESET = 0, SET = !RESET} FlagStatus, ITStatus; From 2e801fbe026b3d2658d504553eb11b262670ba75 Mon Sep 17 00:00:00 2001 From: Moto Moto Date: Sun, 4 Dec 2016 16:04:09 -0600 Subject: [PATCH 07/20] Adjustments to MOTOLAB target for v3.1 --- src/main/target/MOTOLAB/target.c | 2 +- src/main/target/MOTOLAB/target.h | 12 +----------- src/main/target/MOTOLAB/target.mk | 4 +--- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index c9adecf16f..414404fbbe 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -34,6 +34,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1 ), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1 ), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0 ), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0 ), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0 ), // LED - PB8 - *TIM16_CH1, TIM4_CH3, TIM8_CH2 }; diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 295bab37da..d2fe6f1df9 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -34,7 +34,7 @@ #define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL -//#define ENSURE_MPU_DATA_READY_IS_LOW +#define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO #define ACC @@ -55,12 +55,6 @@ #define MPU6000_CS_PIN PB12 #define MPU6000_SPI_INSTANCE SPI2 -//#define BARO -//#define USE_BARO_MS5611 - -//#define MAG -//#define USE_MAG_HMC5883 - #define USE_VCP #define USE_UART1 #define USE_UART2 @@ -87,7 +81,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) #define SENSORS_SET (SENSOR_ACC) #undef GPS @@ -115,9 +108,6 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -// #define TARGET_IO_PORTF (BIT(0)|BIT(1)) -// !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/MOTOLAB/target.mk b/src/main/target/MOTOLAB/target.mk index 35a228d5bd..a464efbde7 100644 --- a/src/main/target/MOTOLAB/target.mk +++ b/src/main/target/MOTOLAB/target.mk @@ -4,7 +4,5 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c + drivers/accgyro_spi_mpu6000.c From 60e22273962db23d31e3ca1dcea4e25309188e5f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 4 Dec 2016 21:48:29 +0000 Subject: [PATCH 08/20] Moved alignment from sensor into device --- src/main/config/config_master.h | 2 - src/main/drivers/accgyro.h | 2 + src/main/drivers/compass.h | 1 + src/main/drivers/sensor.h | 12 +++++ src/main/fc/config.c | 21 ++------ src/main/fc/config.h | 1 - src/main/fc/fc_msp.c | 12 ++--- src/main/io/serial_cli.c | 6 +-- src/main/main.c | 8 ++-- src/main/sensors/acceleration.c | 2 +- src/main/sensors/acceleration.h | 2 +- src/main/sensors/boardalignment.c | 2 +- src/main/sensors/compass.c | 2 +- src/main/sensors/compass.h | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 +- src/main/sensors/initialisation.c | 79 +++++++++++++++---------------- src/main/sensors/initialisation.h | 10 ++-- src/main/sensors/sensors.h | 18 ------- 19 files changed, 82 insertions(+), 104 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index f07a522d2c..f55f17ab06 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -67,7 +67,6 @@ #define servoMixerConfig(x) (&masterConfig.servoMixerConfig) #define gimbalConfig(x) (&masterConfig.gimbalConfig) #define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig) -#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig) #define sensorTrims(x) (&masterConfig.sensorTrims) #define boardAlignment(x) (&masterConfig.boardAlignment) #define imuConfig(x) (&masterConfig.imuConfig) @@ -124,7 +123,6 @@ typedef struct master_s { // global sensor-related stuff sensorSelectionConfig_t sensorSelectionConfig; - sensorAlignmentConfig_t sensorAlignmentConfig; sensorTrims_t sensorTrims; boardAlignment_t boardAlignment; diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 372273ef58..9dbd6f6dd2 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -42,6 +42,7 @@ typedef struct gyroDev_s { volatile bool dataReady; uint16_t lpf; int16_t gyroADCRaw[XYZ_AXIS_COUNT]; + sensor_align_e gyroAlign; } gyroDev_t; typedef struct accDev_s { @@ -49,4 +50,5 @@ typedef struct accDev_s { sensorReadFuncPtr read; // read 3 axis data function uint16_t acc_1G; char revisionCode; // a revision code for the sensor, if known + sensor_align_e accAlign; } accDev_t; diff --git a/src/main/drivers/compass.h b/src/main/drivers/compass.h index 019e654173..f1bfbd9d3b 100644 --- a/src/main/drivers/compass.h +++ b/src/main/drivers/compass.h @@ -22,6 +22,7 @@ typedef struct magDev_s { sensorInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function + sensor_align_e magAlign; } magDev_t; #ifndef MAG_I2C_INSTANCE diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 3e897f8c1d..480aca7dc2 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -17,6 +17,18 @@ #pragma once +typedef enum { + ALIGN_DEFAULT = 0, // driver-provided alignment + CW0_DEG = 1, + CW90_DEG = 2, + CW180_DEG = 3, + CW270_DEG = 4, + CW0_DEG_FLIP = 5, + CW90_DEG_FLIP = 6, + CW180_DEG_FLIP = 7, + CW270_DEG_FLIP = 8 +} sensor_align_e; + struct accDev_s; typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 06102c3603..25c6e26f6a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -230,13 +230,6 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) } #endif -void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) -{ - sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT; - sensorAlignmentConfig->acc_align = ALIGN_DEFAULT; - sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; -} - #ifdef LED_STRIP void resetLedStripConfig(ledStripConfig_t *ledStripConfig) { @@ -589,7 +582,9 @@ void createDefaultConfig(master_t *config) resetAccelerometerTrims(&config->sensorTrims.accZero); - resetSensorAlignment(&config->sensorAlignmentConfig); + config->gyroConfig.gyro_align = ALIGN_DEFAULT; + config->accelerometerConfig.acc_align = ALIGN_DEFAULT; + config->compassConfig.mag_align = ALIGN_DEFAULT; config->boardAlignment.rollDegrees = 0; config->boardAlignment.pitchDegrees = 0; @@ -1004,13 +999,6 @@ void validateAndFixGyroConfig(void) } } -void readEEPROMAndNotify(void) -{ - // re-read written data - readEEPROM(); - beeperConfirmationBeeps(1); -} - void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { @@ -1029,7 +1017,8 @@ void resetEEPROM(void) void saveConfigAndNotify(void) { writeEEPROM(); - readEEPROMAndNotify(); + readEEPROM(); + beeperConfirmationBeeps(1); } void changeProfile(uint8_t profileIndex) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 487cad9dc4..c5fe4ecf60 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -70,7 +70,6 @@ void setPreferredBeeperOffMask(uint32_t mask); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); void resetEEPROM(void); -void readEEPROMAndNotify(void); void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bda80ccc8f..327a87d691 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1079,9 +1079,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SENSOR_ALIGNMENT: - sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align); - sbufWriteU8(dst, sensorAlignmentConfig()->acc_align); - sbufWriteU8(dst, sensorAlignmentConfig()->mag_align); + sbufWriteU8(dst, gyroConfig()->gyro_align); + sbufWriteU8(dst, accelerometerConfig()->acc_align); + sbufWriteU8(dst, compassConfig()->mag_align); break; case MSP_ADVANCED_CONFIG: @@ -1432,9 +1432,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_ALIGNMENT: - sensorAlignmentConfig()->gyro_align = sbufReadU8(src); - sensorAlignmentConfig()->acc_align = sbufReadU8(src); - sensorAlignmentConfig()->mag_align = sbufReadU8(src); + gyroConfig()->gyro_align = sbufReadU8(src); + accelerometerConfig()->acc_align = sbufReadU8(src); + compassConfig()->mag_align = sbufReadU8(src); break; case MSP_SET_ADVANCED_CONFIG: diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f7a9bdd923..0a87607089 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -803,9 +803,9 @@ const clivalue_t valueTable[] = { { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, - { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, - { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, - { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, + { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, + { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, + { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDegrees, .config.minmax = { -180, 360 } }, { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } }, diff --git a/src/main/main.c b/src/main/main.c index ed25d80452..b5ff03742c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -422,10 +422,10 @@ void init(void) #else const void *sonarConfig = NULL; #endif - if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, - &masterConfig.sensorSelectionConfig, - compassConfig()->mag_declination, - &masterConfig.gyroConfig, + if (!sensorsAutodetect(sensorSelectionConfig(), + gyroConfig(), + accelerometerConfig(), + compassConfig(), sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 11ba145e85..13484e40c7 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -215,7 +215,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) } } - alignSensors(acc.accSmooth, acc.accAlign); + alignSensors(acc.accSmooth, acc.dev.accAlign); if (!isAccelerationCalibrationComplete()) { performAcclerationCalibration(rollAndPitchTrims); diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 811767a6b6..e42dd21be6 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -37,7 +37,6 @@ typedef enum { typedef struct acc_s { accDev_t dev; - sensor_align_e accAlign; uint32_t accSamplingInterval; int32_t accSmooth[XYZ_AXIS_COUNT]; } acc_t; @@ -57,6 +56,7 @@ typedef union rollAndPitchTrims_u { typedef struct accelerometerConfig_s { uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz + sensor_align_e acc_align; // acc alignment } accelerometerConfig_t; void accInit(uint32_t gyroTargetLooptime); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 4221d3b488..c4f1988274 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -23,7 +23,7 @@ #include "common/maths.h" #include "common/axis.h" -#include "sensors.h" +#include "drivers/sensor.h" #include "boardalignment.h" diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index e7e500c49e..050baf7b0b 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -60,7 +60,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero) mag.dev.read(magADCRaw); for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis]; // int32_t copy to work with - alignSensors(mag.magADC, mag.magAlign); + alignSensors(mag.magADC, mag.dev.magAlign); if (STATE(CALIBRATE_MAG)) { tCal = currentTime; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index a913174c55..a458c240d5 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -33,7 +33,6 @@ typedef enum { typedef struct mag_s { magDev_t dev; - sensor_align_e magAlign; int32_t magADC[XYZ_AXIS_COUNT]; float magneticDeclination; } mag_t; @@ -43,6 +42,7 @@ extern mag_t mag; typedef struct compassConfig_s { int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. + sensor_align_e mag_align; // mag alignment } compassConfig_t; void compassInit(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f7fd9db019..e6a0a4d7d3 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -181,7 +181,7 @@ void gyroUpdate(void) gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; - alignSensors(gyroADC, gyro.gyroAlign); + alignSensors(gyroADC, gyro.dev.gyroAlign); const bool calibrationComplete = isGyroCalibrationComplete(); if (!calibrationComplete) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 8e9d988493..a532a016a7 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -37,13 +37,13 @@ typedef enum { typedef struct gyro_s { gyroDev_t dev; uint32_t targetLooptime; - sensor_align_e gyroAlign; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; extern gyro_t gyro; typedef struct gyroConfig_s { + sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyro_sync_denom; // Gyro sample divider uint8_t gyro_soft_lpf_type; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 97fd007540..45d714658d 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -161,7 +161,7 @@ bool detectGyro(void) { gyroSensor_e gyroHardware = GYRO_DEFAULT; - gyro.gyroAlign = ALIGN_DEFAULT; + gyro.dev.gyroAlign = ALIGN_DEFAULT; switch(gyroHardware) { case GYRO_DEFAULT: @@ -171,7 +171,7 @@ bool detectGyro(void) if (mpu6050GyroDetect(&gyro.dev)) { gyroHardware = GYRO_MPU6050; #ifdef GYRO_MPU6050_ALIGN - gyro.gyroAlign = GYRO_MPU6050_ALIGN; + gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN; #endif break; } @@ -182,7 +182,7 @@ bool detectGyro(void) if (l3g4200dDetect(&gyro.dev)) { gyroHardware = GYRO_L3G4200D; #ifdef GYRO_L3G4200D_ALIGN - gyro.gyroAlign = GYRO_L3G4200D_ALIGN; + gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN; #endif break; } @@ -194,7 +194,7 @@ bool detectGyro(void) if (mpu3050Detect(&gyro.dev)) { gyroHardware = GYRO_MPU3050; #ifdef GYRO_MPU3050_ALIGN - gyro.gyroAlign = GYRO_MPU3050_ALIGN; + gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN; #endif break; } @@ -206,7 +206,7 @@ bool detectGyro(void) if (l3gd20Detect(&gyro.dev)) { gyroHardware = GYRO_L3GD20; #ifdef GYRO_L3GD20_ALIGN - gyro.gyroAlign = GYRO_L3GD20_ALIGN; + gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN; #endif break; } @@ -218,7 +218,7 @@ bool detectGyro(void) if (mpu6000SpiGyroDetect(&gyro.dev)) { gyroHardware = GYRO_MPU6000; #ifdef GYRO_MPU6000_ALIGN - gyro.gyroAlign = GYRO_MPU6000_ALIGN; + gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN; #endif break; } @@ -235,7 +235,7 @@ bool detectGyro(void) { gyroHardware = GYRO_MPU6500; #ifdef GYRO_MPU6500_ALIGN - gyro.gyroAlign = GYRO_MPU6500_ALIGN; + gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN; #endif break; @@ -250,7 +250,7 @@ bool detectGyro(void) { gyroHardware = GYRO_MPU9250; #ifdef GYRO_MPU9250_ALIGN - gyro.gyroAlign = GYRO_MPU9250_ALIGN; + gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN; #endif break; @@ -264,7 +264,7 @@ bool detectGyro(void) { gyroHardware = GYRO_ICM20689; #ifdef GYRO_ICM20689_ALIGN - gyro.gyroAlign = GYRO_ICM20689_ALIGN; + gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN; #endif break; @@ -303,7 +303,7 @@ static bool detectAcc(accelerationSensor_e accHardwareToUse) #endif retry: - acc.accAlign = ALIGN_DEFAULT; + acc.dev.accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: @@ -318,7 +318,7 @@ retry: if (adxl345Detect(&acc_params, &acc.dev)) { #endif #ifdef ACC_ADXL345_ALIGN - acc.accAlign = ACC_ADXL345_ALIGN; + acc.dev.accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; break; @@ -329,7 +329,7 @@ retry: #ifdef USE_ACC_LSM303DLHC if (lsm303dlhcAccDetect(&acc.dev)) { #ifdef ACC_LSM303DLHC_ALIGN - acc.accAlign = ACC_LSM303DLHC_ALIGN; + acc.dev.accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; break; @@ -340,7 +340,7 @@ retry: #ifdef USE_ACC_MPU6050 if (mpu6050AccDetect(&acc.dev)) { #ifdef ACC_MPU6050_ALIGN - acc.accAlign = ACC_MPU6050_ALIGN; + acc.dev.accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; break; @@ -356,7 +356,7 @@ retry: if (mma8452Detect(&acc.dev)) { #endif #ifdef ACC_MMA8452_ALIGN - acc.accAlign = ACC_MMA8452_ALIGN; + acc.dev.accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; break; @@ -367,7 +367,7 @@ retry: #ifdef USE_ACC_BMA280 if (bma280Detect(&acc.dev)) { #ifdef ACC_BMA280_ALIGN - acc.accAlign = ACC_BMA280_ALIGN; + acc.dev.accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; break; @@ -378,7 +378,7 @@ retry: #ifdef USE_ACC_SPI_MPU6000 if (mpu6000SpiAccDetect(&acc.dev)) { #ifdef ACC_MPU6000_ALIGN - acc.accAlign = ACC_MPU6000_ALIGN; + acc.dev.accAlign = ACC_MPU6000_ALIGN; #endif accHardware = ACC_MPU6000; break; @@ -394,7 +394,7 @@ retry: #endif { #ifdef ACC_MPU6500_ALIGN - acc.accAlign = ACC_MPU6500_ALIGN; + acc.dev.accAlign = ACC_MPU6500_ALIGN; #endif accHardware = ACC_MPU6500; break; @@ -407,7 +407,7 @@ retry: if (icm20689SpiAccDetect(&acc.dev)) { #ifdef ACC_ICM20689_ALIGN - acc.accAlign = ACC_ICM20689_ALIGN; + acc.dev.accAlign = ACC_ICM20689_ALIGN; #endif accHardware = ACC_ICM20689; break; @@ -547,7 +547,7 @@ static bool detectMag(magSensor_e magHardwareToUse) retry: - mag.magAlign = ALIGN_DEFAULT; + mag.dev.magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { case MAG_DEFAULT: @@ -557,7 +557,7 @@ retry: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag.dev, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN - mag.magAlign = MAG_HMC5883_ALIGN; + mag.dev.magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; @@ -569,7 +569,7 @@ retry: #ifdef USE_MAG_AK8975 if (ak8975Detect(&mag.dev)) { #ifdef MAG_AK8975_ALIGN - mag.magAlign = MAG_AK8975_ALIGN; + mag.dev.magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; @@ -581,7 +581,7 @@ retry: #ifdef USE_MAG_AK8963 if (ak8963Detect(&mag.dev)) { #ifdef MAG_AK8963_ALIGN - mag.magAlign = MAG_AK8963_ALIGN; + mag.dev.magAlign = MAG_AK8963_ALIGN; #endif magHardware = MAG_AK8963; break; @@ -623,23 +623,10 @@ static bool detectSonar(void) } #endif -static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig) -{ - if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { - gyro.gyroAlign = sensorAlignmentConfig->gyro_align; - } - if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) { - acc.accAlign = sensorAlignmentConfig->acc_align; - } - if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { - mag.magAlign = sensorAlignmentConfig->mag_align; - } -} - -bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, - const sensorSelectionConfig_t *sensorSelectionConfig, - int16_t magDeclinationFromConfig, +bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, const gyroConfig_t *gyroConfig, + const accelerometerConfig_t *accConfig, + const compassConfig_t *compassConfig, const sonarConfig_t *sonarConfig) { memset(&acc, 0, sizeof(acc)); @@ -676,13 +663,13 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c if (detectMag(sensorSelectionConfig->mag_hardware)) { // calculate magnetic declination - const int16_t deg = magDeclinationFromConfig / 100; - const int16_t min = magDeclinationFromConfig % 100; + const int16_t deg = compassConfig->mag_declination / 100; + const int16_t min = compassConfig->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units compassInit(); } #else - UNUSED(magDeclinationFromConfig); + UNUSED(compassConfig); #endif #ifdef BARO @@ -697,7 +684,15 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, UNUSED(sonarConfig); #endif - reconfigureAlignment(sensorAlignmentConfig); + if (gyroConfig->gyro_align != ALIGN_DEFAULT) { + gyro.dev.gyroAlign = gyroConfig->gyro_align; + } + if (accConfig->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accConfig->acc_align; + } + if (compassConfig->mag_align != ALIGN_DEFAULT) { + mag.dev.magAlign = compassConfig->mag_align; + } return true; } diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index c1b3204f91..b0268c74f7 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -21,8 +21,8 @@ struct sensorAlignmentConfig_s; struct sensorSelectionConfig_s; struct gyroConfig_s; struct sonarConfig_s; -bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig, - const struct sensorSelectionConfig_s *sensorSelectionConfig, - int16_t magDeclinationFromConfig, - const struct gyroConfig_s *gyroConfig, - const struct sonarConfig_s *sonarConfig); +bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, + const gyroConfig_t *gyroConfig, + const accelerometerConfig_t *accConfig, + const compassConfig_t *compassConfig, + const sonarConfig_t *sonarConfig); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 107a74c75d..1f7215f70f 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -52,24 +52,6 @@ typedef enum { SENSOR_GPSMAG = 1 << 6 } sensors_e; -typedef enum { - ALIGN_DEFAULT = 0, // driver-provided alignment - CW0_DEG = 1, - CW90_DEG = 2, - CW180_DEG = 3, - CW270_DEG = 4, - CW0_DEG_FLIP = 5, - CW90_DEG_FLIP = 6, - CW180_DEG_FLIP = 7, - CW270_DEG_FLIP = 8 -} sensor_align_e; - -typedef struct sensorAlignmentConfig_s { - sensor_align_e gyro_align; // gyro alignment - sensor_align_e acc_align; // acc alignment - sensor_align_e mag_align; // mag alignment -} sensorAlignmentConfig_t; - typedef struct sensorSelectionConfig_s { uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t baro_hardware; // Barometer hardware to use From 06e871406ed5742b5e5e30c565d11275b602c5df Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 4 Dec 2016 22:15:41 +0000 Subject: [PATCH 09/20] Moved xx_hardware out of sensorSelectionConfig into config for specific sensor --- src/main/blackbox/blackbox.c | 6 +++--- src/main/config/config_master.h | 2 -- src/main/fc/config.c | 6 +++--- src/main/fc/fc_msp.c | 12 ++++++------ src/main/io/serial_cli.c | 6 +++--- src/main/main.c | 6 +----- src/main/sensors/acceleration.h | 1 + src/main/sensors/barometer.h | 1 + src/main/sensors/compass.h | 1 + src/main/sensors/initialisation.c | 18 ++++++++++-------- src/main/sensors/initialisation.h | 8 ++------ src/main/sensors/sensors.h | 6 ------ 12 files changed, 31 insertions(+), 42 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d6ac70dc94..dcae209230 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1271,9 +1271,9 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, gyroConfig()->gyro_soft_notch_cutoff_2); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", sensorSelectionConfig()->acc_hardware); - BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", sensorSelectionConfig()->baro_hardware); - BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", sensorSelectionConfig()->mag_hardware); + BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", accelerometerConfig()->acc_hardware); + BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", barometerConfig()->baro_hardware); + BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", compassConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", armingConfig()->gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", rxConfig()->rcInterpolation); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", rxConfig()->rcInterpolationInterval); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index f55f17ab06..4674e1bff7 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -66,7 +66,6 @@ #define servoConfig(x) (&masterConfig.servoConfig) #define servoMixerConfig(x) (&masterConfig.servoMixerConfig) #define gimbalConfig(x) (&masterConfig.gimbalConfig) -#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig) #define sensorTrims(x) (&masterConfig.sensorTrims) #define boardAlignment(x) (&masterConfig.boardAlignment) #define imuConfig(x) (&masterConfig.imuConfig) @@ -122,7 +121,6 @@ typedef struct master_s { #endif // global sensor-related stuff - sensorSelectionConfig_t sensorSelectionConfig; sensorTrims_t sensorTrims; boardAlignment_t boardAlignment; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 25c6e26f6a..18d6f5492d 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -589,15 +589,15 @@ void createDefaultConfig(master_t *config) config->boardAlignment.rollDegrees = 0; config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; - config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT; // default/autodetect + config->accelerometerConfig.acc_hardware = ACC_DEFAULT; // default/autodetect config->max_angle_inclination = 700; // 70 degrees config->rcControlsConfig.yaw_control_direction = 1; config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - config->sensorSelectionConfig.mag_hardware = 1; + config->compassConfig.mag_hardware = 1; - config->sensorSelectionConfig.baro_hardware = 1; + config->barometerConfig.baro_hardware = 1; resetBatteryConfig(&config->batteryConfig); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 327a87d691..4eab72f527 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1125,9 +1125,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SENSOR_CONFIG: - sbufWriteU8(dst, sensorSelectionConfig()->acc_hardware); - sbufWriteU8(dst, sensorSelectionConfig()->baro_hardware); - sbufWriteU8(dst, sensorSelectionConfig()->mag_hardware); + sbufWriteU8(dst, accelerometerConfig()->acc_hardware); + sbufWriteU8(dst, barometerConfig()->baro_hardware); + sbufWriteU8(dst, compassConfig()->mag_hardware); break; case MSP_REBOOT: @@ -1487,9 +1487,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - sensorSelectionConfig()->acc_hardware = sbufReadU8(src); - sensorSelectionConfig()->baro_hardware = sbufReadU8(src); - sensorSelectionConfig()->mag_hardware = sbufReadU8(src); + accelerometerConfig()->acc_hardware = sbufReadU8(src); + barometerConfig()->baro_hardware = sbufReadU8(src); + compassConfig()->mag_hardware = sbufReadU8(src); break; case MSP_RESET_CONF: diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0a87607089..cfc0cfe750 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -869,7 +869,7 @@ const clivalue_t valueTable[] = { { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &rxConfig()->rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &rxConfig()->rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, - { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, + { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &accelerometerConfig()->acc_lpf_hz, .config.minmax = { 0, 400 } }, { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.xy, .config.minmax = { 0, 100 } }, { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.z, .config.minmax = { 0, 100 } }, @@ -882,11 +882,11 @@ const clivalue_t valueTable[] = { { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_noise_lpf, .config.minmax = { 0 , 1 } }, { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_vel, .config.minmax = { 0 , 1 } }, { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_alt, .config.minmax = { 0 , 1 } }, - { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, + { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &barometerConfig()->baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, #endif #ifdef MAG - { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, + { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } }, #endif { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, diff --git a/src/main/main.c b/src/main/main.c index b5ff03742c..cf1060a547 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -422,11 +422,7 @@ void init(void) #else const void *sonarConfig = NULL; #endif - if (!sensorsAutodetect(sensorSelectionConfig(), - gyroConfig(), - accelerometerConfig(), - compassConfig(), - sonarConfig)) { + if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index e42dd21be6..e22f75c658 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -57,6 +57,7 @@ typedef union rollAndPitchTrims_u { typedef struct accelerometerConfig_s { uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz sensor_align_e acc_align; // acc alignment + uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device } accelerometerConfig_t; void accInit(uint32_t gyroTargetLooptime); diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index fdd47e5f99..53025fc4cd 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -30,6 +30,7 @@ typedef enum { #define BARO_SAMPLE_COUNT_MAX 48 typedef struct barometerConfig_s { + uint8_t baro_hardware; // Barometer hardware to use uint8_t baro_sample_count; // size of baro filter array float baro_noise_lpf; // additional LPF to reduce baro noise float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index a458c240d5..aab1314fdd 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -43,6 +43,7 @@ typedef struct compassConfig_s { int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. sensor_align_e mag_align; // mag alignment + uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device } compassConfig_t; void compassInit(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 45d714658d..60322c4cc4 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -623,10 +623,10 @@ static bool detectSonar(void) } #endif -bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, - const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accConfig, +bool sensorsAutodetect(const gyroConfig_t *gyroConfig, + const accelerometerConfig_t *accelerometerConfig, const compassConfig_t *compassConfig, + const barometerConfig_t *barometerConfig, const sonarConfig_t *sonarConfig) { memset(&acc, 0, sizeof(acc)); @@ -651,7 +651,7 @@ bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, gyro.dev.init(&gyro.dev); // driver initialisation gyroInit(gyroConfig); // sensor initialisation - if (detectAcc(sensorSelectionConfig->acc_hardware)) { + if (detectAcc(accelerometerConfig->acc_hardware)) { acc.dev.acc_1G = 256; // set default acc.dev.init(&acc.dev); // driver initialisation accInit(gyro.targetLooptime); // sensor initialisation @@ -661,7 +661,7 @@ bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c - if (detectMag(sensorSelectionConfig->mag_hardware)) { + if (detectMag(compassConfig->mag_hardware)) { // calculate magnetic declination const int16_t deg = compassConfig->mag_declination / 100; const int16_t min = compassConfig->mag_declination % 100; @@ -673,7 +673,9 @@ bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, #endif #ifdef BARO - detectBaro(sensorSelectionConfig->baro_hardware); + detectBaro(barometerConfig->baro_hardware); +#else + UNUSED(barometerConfig); #endif #ifdef SONAR @@ -687,8 +689,8 @@ bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, if (gyroConfig->gyro_align != ALIGN_DEFAULT) { gyro.dev.gyroAlign = gyroConfig->gyro_align; } - if (accConfig->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accConfig->acc_align; + if (accelerometerConfig->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig->acc_align; } if (compassConfig->mag_align != ALIGN_DEFAULT) { mag.dev.magAlign = compassConfig->mag_align; diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index b0268c74f7..e656294c6f 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,12 +17,8 @@ #pragma once -struct sensorAlignmentConfig_s; -struct sensorSelectionConfig_s; -struct gyroConfig_s; -struct sonarConfig_s; -bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig, - const gyroConfig_t *gyroConfig, +bool sensorsAutodetect(const gyroConfig_t *gyroConfig, const accelerometerConfig_t *accConfig, const compassConfig_t *compassConfig, + const barometerConfig_t *baroConfig, const sonarConfig_t *sonarConfig); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 1f7215f70f..aca5aa74e2 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -52,12 +52,6 @@ typedef enum { SENSOR_GPSMAG = 1 << 6 } sensors_e; -typedef struct sensorSelectionConfig_s { - uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device - uint8_t baro_hardware; // Barometer hardware to use - uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device -} sensorSelectionConfig_t; - typedef struct sensorTrims_s { flightDynamicsTrims_t accZero; flightDynamicsTrims_t magZero; From 229f6d14d4108806a24c5d6b90ac04f8e61ff86e Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 4 Dec 2016 22:25:42 +0000 Subject: [PATCH 10/20] Moved accZero and magZero out of sensorTrims into acc and compass config --- src/main/config/config_master.h | 3 --- src/main/fc/config.c | 4 ++-- src/main/fc/fc_tasks.c | 2 +- src/main/io/serial_cli.c | 6 +++--- src/main/sensors/acceleration.h | 1 + src/main/sensors/compass.h | 1 + src/main/sensors/sensors.h | 5 ----- 7 files changed, 8 insertions(+), 14 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4674e1bff7..cc30a69749 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -66,7 +66,6 @@ #define servoConfig(x) (&masterConfig.servoConfig) #define servoMixerConfig(x) (&masterConfig.servoMixerConfig) #define gimbalConfig(x) (&masterConfig.gimbalConfig) -#define sensorTrims(x) (&masterConfig.sensorTrims) #define boardAlignment(x) (&masterConfig.boardAlignment) #define imuConfig(x) (&masterConfig.imuConfig) #define gyroConfig(x) (&masterConfig.gyroConfig) @@ -120,8 +119,6 @@ typedef struct master_s { gimbalConfig_t gimbalConfig; #endif - // global sensor-related stuff - sensorTrims_t sensorTrims; boardAlignment_t boardAlignment; imuConfig_t imuConfig; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 18d6f5492d..fa1cc67cdc 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -580,7 +580,7 @@ void createDefaultConfig(master_t *config) config->debug_mode = DEBUG_MODE; - resetAccelerometerTrims(&config->sensorTrims.accZero); + resetAccelerometerTrims(&config->accelerometerConfig.accZero); config->gyroConfig.gyro_align = ALIGN_DEFAULT; config->accelerometerConfig.acc_align = ALIGN_DEFAULT; @@ -837,7 +837,7 @@ void activateConfig(void) #endif useFailsafeConfig(&masterConfig.failsafeConfig); - setAccelerationTrims(&sensorTrims()->accZero); + setAccelerationTrims(&accelerometerConfig()->accZero); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); mixerUseConfigs( diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a34e1666c6..f043290f61 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -158,7 +158,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) static void taskUpdateCompass(timeUs_t currentTimeUs) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTimeUs, &sensorTrims()->magZero); + compassUpdate(currentTimeUs, &compassConfig()->magZero); } } #endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index cfc0cfe750..607c2ffdb3 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -945,9 +945,9 @@ const clivalue_t valueTable[] = { #endif #ifdef MAG - { "magzero_x", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[X], .config.minmax = { -32768, 32767 } }, - { "magzero_y", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Y], .config.minmax = { -32768, 32767 } }, - { "magzero_z", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Z], .config.minmax = { -32768, 32767 } }, + { "magzero_x", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[X], .config.minmax = { -32768, 32767 } }, + { "magzero_y", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[Y], .config.minmax = { -32768, 32767 } }, + { "magzero_z", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[Z], .config.minmax = { -32768, 32767 } }, #endif #ifdef LED_STRIP { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index e22f75c658..88f391a5c8 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -58,6 +58,7 @@ typedef struct accelerometerConfig_s { uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz sensor_align_e acc_align; // acc alignment uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device + flightDynamicsTrims_t accZero; } accelerometerConfig_t; void accInit(uint32_t gyroTargetLooptime); diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index aab1314fdd..08ce80b300 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -44,6 +44,7 @@ typedef struct compassConfig_s { // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. sensor_align_e mag_align; // mag alignment uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device + flightDynamicsTrims_t magZero; } compassConfig_t; void compassInit(void); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index aca5aa74e2..a93446511a 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -51,8 +51,3 @@ typedef enum { SENSOR_GPS = 1 << 5, SENSOR_GPSMAG = 1 << 6 } sensors_e; - -typedef struct sensorTrims_s { - flightDynamicsTrims_t accZero; - flightDynamicsTrims_t magZero; -} sensorTrims_t; From f4cd683ecd08617e93aafb211b0639a139af790a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 05:22:07 +0000 Subject: [PATCH 11/20] Renamed sensor detect functions from eg detectGyro to gyroDetect --- src/main/sensors/initialisation.c | 76 +++++++++++++++---------------- 1 file changed, 38 insertions(+), 38 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 60322c4cc4..bfb6127046 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -157,7 +157,7 @@ bool fakeAccDetect(accDev_t *acc) } #endif -bool detectGyro(void) +bool gyroDetect(gyroDev_t *dev) { gyroSensor_e gyroHardware = GYRO_DEFAULT; @@ -168,7 +168,7 @@ bool detectGyro(void) ; // fallthrough case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(&gyro.dev)) { + if (mpu6050GyroDetect(dev)) { gyroHardware = GYRO_MPU6050; #ifdef GYRO_MPU6050_ALIGN gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN; @@ -179,7 +179,7 @@ bool detectGyro(void) ; // fallthrough case GYRO_L3G4200D: #ifdef USE_GYRO_L3G4200D - if (l3g4200dDetect(&gyro.dev)) { + if (l3g4200dDetect(dev)) { gyroHardware = GYRO_L3G4200D; #ifdef GYRO_L3G4200D_ALIGN gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN; @@ -191,7 +191,7 @@ bool detectGyro(void) case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 - if (mpu3050Detect(&gyro.dev)) { + if (mpu3050Detect(dev)) { gyroHardware = GYRO_MPU3050; #ifdef GYRO_MPU3050_ALIGN gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN; @@ -203,7 +203,7 @@ bool detectGyro(void) case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 - if (l3gd20Detect(&gyro.dev)) { + if (l3gd20Detect(dev)) { gyroHardware = GYRO_L3GD20; #ifdef GYRO_L3GD20_ALIGN gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN; @@ -215,7 +215,7 @@ bool detectGyro(void) case GYRO_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiGyroDetect(&gyro.dev)) { + if (mpu6000SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU6000; #ifdef GYRO_MPU6000_ALIGN gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN; @@ -228,9 +228,9 @@ bool detectGyro(void) case GYRO_MPU6500: #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) #ifdef USE_GYRO_SPI_MPU6500 - if (mpu6500GyroDetect(&gyro.dev) || mpu6500SpiGyroDetect(&gyro.dev)) + if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) #else - if (mpu6500GyroDetect(&gyro.dev)) + if (mpu6500GyroDetect(dev)) #endif { gyroHardware = GYRO_MPU6500; @@ -246,7 +246,7 @@ bool detectGyro(void) case GYRO_MPU9250: #ifdef USE_GYRO_SPI_MPU9250 - if (mpu9250SpiGyroDetect(&gyro.dev)) + if (mpu9250SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU9250; #ifdef GYRO_MPU9250_ALIGN @@ -260,7 +260,7 @@ bool detectGyro(void) case GYRO_ICM20689: #ifdef USE_GYRO_SPI_ICM20689 - if (icm20689SpiGyroDetect(&gyro.dev)) + if (icm20689SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20689; #ifdef GYRO_ICM20689_ALIGN @@ -274,7 +274,7 @@ bool detectGyro(void) case GYRO_FAKE: #ifdef USE_FAKE_GYRO - if (fakeGyroDetect(&gyro.dev)) { + if (fakeGyroDetect(dev)) { gyroHardware = GYRO_FAKE; break; } @@ -294,7 +294,7 @@ bool detectGyro(void) return true; } -static bool detectAcc(accelerationSensor_e accHardwareToUse) +static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; @@ -313,9 +313,9 @@ retry: acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently #ifdef NAZE - if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc.dev)) { + if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, dev)) { #else - if (adxl345Detect(&acc_params, &acc.dev)) { + if (adxl345Detect(&acc_params, dev)) { #endif #ifdef ACC_ADXL345_ALIGN acc.dev.accAlign = ACC_ADXL345_ALIGN; @@ -327,7 +327,7 @@ retry: ; // fallthrough case ACC_LSM303DLHC: #ifdef USE_ACC_LSM303DLHC - if (lsm303dlhcAccDetect(&acc.dev)) { + if (lsm303dlhcAccDetect(dev)) { #ifdef ACC_LSM303DLHC_ALIGN acc.dev.accAlign = ACC_LSM303DLHC_ALIGN; #endif @@ -338,7 +338,7 @@ retry: ; // fallthrough case ACC_MPU6050: // MPU6050 #ifdef USE_ACC_MPU6050 - if (mpu6050AccDetect(&acc.dev)) { + if (mpu6050AccDetect(dev)) { #ifdef ACC_MPU6050_ALIGN acc.dev.accAlign = ACC_MPU6050_ALIGN; #endif @@ -351,9 +351,9 @@ retry: #ifdef USE_ACC_MMA8452 #ifdef NAZE // Not supported with this frequency - if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc.dev)) { + if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) { #else - if (mma8452Detect(&acc.dev)) { + if (mma8452Detect(dev)) { #endif #ifdef ACC_MMA8452_ALIGN acc.dev.accAlign = ACC_MMA8452_ALIGN; @@ -365,7 +365,7 @@ retry: ; // fallthrough case ACC_BMA280: // BMA280 #ifdef USE_ACC_BMA280 - if (bma280Detect(&acc.dev)) { + if (bma280Detect(dev)) { #ifdef ACC_BMA280_ALIGN acc.dev.accAlign = ACC_BMA280_ALIGN; #endif @@ -376,7 +376,7 @@ retry: ; // fallthrough case ACC_MPU6000: #ifdef USE_ACC_SPI_MPU6000 - if (mpu6000SpiAccDetect(&acc.dev)) { + if (mpu6000SpiAccDetect(dev)) { #ifdef ACC_MPU6000_ALIGN acc.dev.accAlign = ACC_MPU6000_ALIGN; #endif @@ -388,9 +388,9 @@ retry: case ACC_MPU6500: #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 - if (mpu6500AccDetect(&acc.dev) || mpu6500SpiAccDetect(&acc.dev)) + if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) #else - if (mpu6500AccDetect(&acc.dev)) + if (mpu6500AccDetect(dev)) #endif { #ifdef ACC_MPU6500_ALIGN @@ -404,7 +404,7 @@ retry: case ACC_ICM20689: #ifdef USE_ACC_SPI_ICM20689 - if (icm20689SpiAccDetect(&acc.dev)) + if (icm20689SpiAccDetect(dev)) { #ifdef ACC_ICM20689_ALIGN acc.dev.accAlign = ACC_ICM20689_ALIGN; @@ -416,7 +416,7 @@ retry: ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC - if (fakeAccDetect(&acc.dev)) { + if (fakeAccDetect(dev)) { accHardware = ACC_FAKE; break; } @@ -446,7 +446,7 @@ retry: } #ifdef BARO -static bool detectBaro(baroSensor_e baroHardwareToUse) +static bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function @@ -476,7 +476,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse) ; // fallthough case BARO_BMP085: #ifdef USE_BARO_BMP085 - if (bmp085Detect(bmp085Config, &baro.dev)) { + if (bmp085Detect(bmp085Config, dev)) { baroHardware = BARO_BMP085; break; } @@ -484,7 +484,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse) ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 - if (ms5611Detect(&baro.dev)) { + if (ms5611Detect(dev)) { baroHardware = BARO_MS5611; break; } @@ -492,7 +492,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse) ; // fallthough case BARO_BMP280: #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) - if (bmp280Detect(&baro.dev)) { + if (bmp280Detect(dev)) { baroHardware = BARO_BMP280; break; } @@ -514,7 +514,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse) #endif #ifdef MAG -static bool detectMag(magSensor_e magHardwareToUse) +static bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) { magSensor_e magHardware; @@ -555,7 +555,7 @@ retry: case MAG_HMC5883: #ifdef USE_MAG_HMC5883 - if (hmc5883lDetect(&mag.dev, hmc5883Config)) { + if (hmc5883lDetect(dev, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN mag.dev.magAlign = MAG_HMC5883_ALIGN; #endif @@ -567,7 +567,7 @@ retry: case MAG_AK8975: #ifdef USE_MAG_AK8975 - if (ak8975Detect(&mag.dev)) { + if (ak8975Detect(dev)) { #ifdef MAG_AK8975_ALIGN mag.dev.magAlign = MAG_AK8975_ALIGN; #endif @@ -579,7 +579,7 @@ retry: case MAG_AK8963: #ifdef USE_MAG_AK8963 - if (ak8963Detect(&mag.dev)) { + if (ak8963Detect(dev)) { #ifdef MAG_AK8963_ALIGN mag.dev.magAlign = MAG_AK8963_ALIGN; #endif @@ -611,7 +611,7 @@ retry: #endif #ifdef SONAR -static bool detectSonar(void) +static bool sonarDetect(void) { if (feature(FEATURE_SONAR)) { // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, @@ -640,7 +640,7 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, UNUSED(mpuDetectionResult); #endif - if (!detectGyro()) { + if (!gyroDetect(&gyro.dev)) { return false; } @@ -651,7 +651,7 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, gyro.dev.init(&gyro.dev); // driver initialisation gyroInit(gyroConfig); // sensor initialisation - if (detectAcc(accelerometerConfig->acc_hardware)) { + if (accDetect(&acc.dev, accelerometerConfig->acc_hardware)) { acc.dev.acc_1G = 256; // set default acc.dev.init(&acc.dev); // driver initialisation accInit(gyro.targetLooptime); // sensor initialisation @@ -661,7 +661,7 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c - if (detectMag(compassConfig->mag_hardware)) { + if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { // calculate magnetic declination const int16_t deg = compassConfig->mag_declination / 100; const int16_t min = compassConfig->mag_declination % 100; @@ -673,13 +673,13 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig, #endif #ifdef BARO - detectBaro(barometerConfig->baro_hardware); + baroDetect(&baro.dev, barometerConfig->baro_hardware); #else UNUSED(barometerConfig); #endif #ifdef SONAR - if (detectSonar()) { + if (sonarDetect()) { sonarInit(sonarConfig); } #else From dc076c17bdc91660940c2e1d6c847d0c79b4daf3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 07:12:45 +0000 Subject: [PATCH 12/20] Fixed up tests --- src/test/unit/alignsensor_unittest.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc index f618f24fcd..4fcaaecef0 100644 --- a/src/test/unit/alignsensor_unittest.cc +++ b/src/test/unit/alignsensor_unittest.cc @@ -21,6 +21,7 @@ extern "C" { #include "common/axis.h" +#include "drivers/sensor.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" } From 34ee1d06419320c918e131f471dec5359774e1d0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 07:16:39 +0000 Subject: [PATCH 13/20] Fixed up target config.c files --- src/main/target/ALIENFLIGHTF3/config.c | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 2 +- src/main/target/BLUEJAYF4/config.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index a7cfc3a91e..516b64a78a 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -47,7 +47,7 @@ void targetConfiguration(master_t *config) { config->rxConfig.spektrum_sat_bind = 5; config->rxConfig.spektrum_sat_bind_autoreset = 1; - config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default + config->compassConfig.mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.minthrottle = 1000; diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index e4089f74a2..363a9704d3 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -58,7 +58,7 @@ void targetConfiguration(master_t *config) { config->batteryConfig.currentMeterOffset = CURRENTOFFSET; config->batteryConfig.currentMeterScale = CURRENTSCALE; - config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default + config->compassConfig.mag_hardware = MAG_NONE; // disabled by default if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.minthrottle = 1000; diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 00c3fc3515..fc2fcaaf65 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -31,8 +31,8 @@ void targetConfiguration(master_t *config) { if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { - config->sensorAlignmentConfig.gyro_align = CW180_DEG; - config->sensorAlignmentConfig.acc_align = CW180_DEG; + config->gyroConfig.gyro_align = CW180_DEG; + config->accelerometerConfig.acc_align = CW180_DEG; config->beeperConfig.ioTag = IO_TAG(BEEPER_OPT); } From 08a81512056c970df4a441f2e7cb1912bb334ac0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 07:30:23 +0000 Subject: [PATCH 14/20] Fixed up COLIBRI config.c --- src/main/target/COLIBRI/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index e01ae9d851..5ae9168cfe 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -48,7 +48,7 @@ void targetConfiguration(master_t *config) config->boardAlignment.pitchDegrees = 10; //config->rcControlsConfig.deadband = 10; //config->rcControlsConfig.yaw_deadband = 10; - config->sensorSelectionConfig.mag_hardware = 1; + config->compassConfig.mag_hardware = 1; config->profile[0].controlRateProfile[0].dynThrPID = 45; config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700; From 35e4862ddb331b2091e68d4824929558572947c0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 07:48:57 +0000 Subject: [PATCH 15/20] Fixed up MULTIFLIGHT_PICO config.c --- src/main/target/MULTIFLITEPICO/config.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index ddc186ccc7..dfbe0a8fea 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -40,8 +40,9 @@ #include "config/config_master.h" // alternative defaults settings for MULTIFLITEPICO targets -void targetConfiguration(master_t *config) { - config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default +void targetConfiguration(master_t *config) +{ + config->compassConfig.mag_hardware = MAG_NONE; // disabled by default config->batteryConfig.vbatscale = 100; config->batteryConfig.vbatresdivval = 15; From 4bee6193e8b4035bc8b010ceb43ff2ffe17b29b8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 5 Dec 2016 11:03:49 +0000 Subject: [PATCH 16/20] Moved faked sensors into their own driver files --- src/main/drivers/accgyro_fake.c | 110 +++++++++++++++++++++ src/main/drivers/accgyro_fake.h | 26 +++++ src/main/drivers/barometer_fake.c | 70 +++++++++++++ src/main/drivers/barometer_fake.h | 23 +++++ src/main/drivers/compass_fake.c | 66 +++++++++++++ src/main/drivers/compass_fake.h | 22 +++++ src/main/sensors/initialisation.c | 68 +------------ src/main/target/STM32F3DISCOVERY/target.mk | 4 +- 8 files changed, 324 insertions(+), 65 deletions(-) create mode 100644 src/main/drivers/accgyro_fake.c create mode 100644 src/main/drivers/accgyro_fake.h create mode 100644 src/main/drivers/barometer_fake.c create mode 100644 src/main/drivers/barometer_fake.h create mode 100644 src/main/drivers/compass_fake.c create mode 100644 src/main/drivers/compass_fake.h diff --git a/src/main/drivers/accgyro_fake.c b/src/main/drivers/accgyro_fake.c new file mode 100644 index 0000000000..c592f01846 --- /dev/null +++ b/src/main/drivers/accgyro_fake.c @@ -0,0 +1,110 @@ +/* + * 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 "common/axis.h" +#include "common/utils.h" + +#include "accgyro.h" +#include "accgyro_fake.h" + + +#ifdef USE_FAKE_GYRO + +static int16_t fakeGyroADC[XYZ_AXIS_COUNT]; + +static void fakeGyroInit(gyroDev_t *gyro) +{ + UNUSED(gyro); +} + +void fakeGyroSet(int16_t x, int16_t y, int16_t z) +{ + fakeGyroADC[X] = x; + fakeGyroADC[Y] = y; + fakeGyroADC[Z] = z; +} + +static bool fakeGyroRead(gyroDev_t *gyro) +{ + gyro->gyroADCRaw[X] = fakeGyroADC[X]; + gyro->gyroADCRaw[Y] = fakeGyroADC[Y]; + gyro->gyroADCRaw[Z] = fakeGyroADC[Z]; + return true; +} + +static bool fakeGyroReadTemperature(int16_t *tempData) +{ + UNUSED(tempData); + return true; +} + +static bool fakeGyroInitStatus(gyroDev_t *gyro) +{ + UNUSED(gyro); + return true; +} + +bool fakeGyroDetect(gyroDev_t *gyro) +{ + gyro->init = fakeGyroInit; + gyro->intStatus = fakeGyroInitStatus; + gyro->read = fakeGyroRead; + gyro->temperature = fakeGyroReadTemperature; + gyro->scale = 1.0f; + return true; +} +#endif // USE_FAKE_GYRO + + +#ifdef USE_FAKE_ACC + +static int16_t fakeAccData[XYZ_AXIS_COUNT]; + +static void fakeAccInit(accDev_t *acc) +{ + UNUSED(acc); +} + +void fakeAccSet(int16_t x, int16_t y, int16_t z) +{ + fakeAccData[X] = x; + fakeAccData[Y] = y; + fakeAccData[Z] = z; +} + +static bool fakeAccRead(int16_t *accData) +{ + accData[X] = fakeAccData[X]; + accData[Y] = fakeAccData[Y]; + accData[Z] = fakeAccData[Z]; + return true; +} + +bool fakeAccDetect(accDev_t *acc) +{ + acc->init = fakeAccInit; + acc->read = fakeAccRead; + acc->revisionCode = 0; + return true; +} +#endif // USE_FAKE_ACC + diff --git a/src/main/drivers/accgyro_fake.h b/src/main/drivers/accgyro_fake.h new file mode 100644 index 0000000000..d8452916bf --- /dev/null +++ b/src/main/drivers/accgyro_fake.h @@ -0,0 +1,26 @@ +/* + * 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 . + */ + +#pragma once + +struct accDev_s; +bool fakeAccDetect(struct accDev_s *acc); +void fakeAccSet(int16_t x, int16_t y, int16_t z); + +struct gyroDev_s; +bool fakeGyroDetect(struct gyroDev_s *gyro); +void fakeGyroSet(int16_t x, int16_t y, int16_t z); diff --git a/src/main/drivers/barometer_fake.c b/src/main/drivers/barometer_fake.c new file mode 100644 index 0000000000..b567503603 --- /dev/null +++ b/src/main/drivers/barometer_fake.c @@ -0,0 +1,70 @@ +/* + * 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 + +#ifdef USE_FAKE_BARO + +#include "barometer.h" +#include "barometer_fake.h" + + +static int32_t fakePressure; +static int32_t fakeTemperature; + + +static void fakeBaroStartGet(void) +{ +} + +static void fakeBaroCalculate(int32_t *pressure, int32_t *temperature) +{ + if (pressure) + *pressure = fakePressure; + if (temperature) + *temperature = fakeTemperature; +} + +void fakeBaroSet(int32_t pressure, int32_t temperature) +{ + fakePressure = pressure; + fakeTemperature = temperature; +} + +bool fakeBaroDetect(baroDev_t *baro) +{ + fakePressure = 101325; // pressure in Pa (0m MSL) + fakeTemperature = 2500; // temperature in 0.01 C = 25 deg + + // these are dummy as temperature is measured as part of pressure + baro->ut_delay = 10000; + baro->get_ut = fakeBaroStartGet; + baro->start_ut = fakeBaroStartGet; + + // only _up part is executed, and gets both temperature and pressure + baro->up_delay = 10000; + baro->start_up = fakeBaroStartGet; + baro->get_up = fakeBaroStartGet; + baro->calculate = fakeBaroCalculate; + + return true; +} +#endif // USE_FAKE_BARO + diff --git a/src/main/drivers/barometer_fake.h b/src/main/drivers/barometer_fake.h new file mode 100644 index 0000000000..bdf67ed24c --- /dev/null +++ b/src/main/drivers/barometer_fake.h @@ -0,0 +1,23 @@ +/* + * 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 . + */ + +#pragma once + +struct baroDev_s; +bool fakeBaroDetect(struct baroDev_s *baro); +void fakeBaroSet(int32_t pressure, int32_t temperature); + diff --git a/src/main/drivers/compass_fake.c b/src/main/drivers/compass_fake.c new file mode 100644 index 0000000000..cb8098c77d --- /dev/null +++ b/src/main/drivers/compass_fake.c @@ -0,0 +1,66 @@ +/* + * 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" + +#ifdef USE_FAKE_MAG + +#include "build/build_config.h" + +#include "common/axis.h" + +#include "compass.h" +#include "compass_fake.h" + + +static int16_t fakeMagData[XYZ_AXIS_COUNT]; + +static bool fakeMagInit(void) +{ + // initially point north + fakeMagData[X] = 4096; + fakeMagData[Y] = 0; + fakeMagData[Z] = 0; + return true; +} + +void fakeMagSet(int16_t x, int16_t y, int16_t z) +{ + fakeMagData[X] = x; + fakeMagData[Y] = y; + fakeMagData[Z] = z; +} + +static bool fakeMagRead(int16_t *magData) +{ + magData[X] = fakeMagData[X]; + magData[Y] = fakeMagData[Y]; + magData[Z] = fakeMagData[Z]; + return true; +} + +bool fakeMagDetect(magDev_t *mag) +{ + mag->init = fakeMagInit; + mag->read = fakeMagRead; + return true; +} +#endif // USE_FAKE_MAG + diff --git a/src/main/drivers/compass_fake.h b/src/main/drivers/compass_fake.h new file mode 100644 index 0000000000..81957c3563 --- /dev/null +++ b/src/main/drivers/compass_fake.h @@ -0,0 +1,22 @@ +/* + * 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 . + */ + +#pragma once + +struct magDev_s; +bool fakeMagDetect(struct magDev_s *mag); +void fakeMagSet(int16_t x, int16_t y, int16_t z); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bfb6127046..9116c18374 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -35,6 +35,7 @@ #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" +#include "drivers/accgyro_fake.h" #include "drivers/accgyro_l3g4200d.h" #include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mpu.h" @@ -54,12 +55,14 @@ #include "drivers/barometer.h" #include "drivers/barometer_bmp085.h" #include "drivers/barometer_bmp280.h" +#include "drivers/barometer_fake.h" #include "drivers/barometer_ms5611.h" #include "drivers/compass.h" -#include "drivers/compass_hmc5883l.h" #include "drivers/compass_ak8975.h" #include "drivers/compass_ak8963.h" +#include "drivers/compass_fake.h" +#include "drivers/compass_hmc5883l.h" #include "drivers/sonar_hcsr04.h" @@ -94,69 +97,6 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #endif } -#ifdef USE_FAKE_GYRO -int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 }; -static void fakeGyroInit(gyroDev_t *gyro) -{ - UNUSED(gyro); -} - -static bool fakeGyroRead(gyroDev_t *gyro) -{ - for (int i = 0; i < XYZ_AXIS_COUNT; ++i) { - gyro->gyroADCRaw[X] = fake_gyro_values[i]; - } - - return true; -} - -static bool fakeGyroReadTemp(int16_t *tempData) -{ - UNUSED(tempData); - return true; -} - - -static bool fakeGyroInitStatus(gyroDev_t *gyro) -{ - UNUSED(gyro); - return true; -} - -bool fakeGyroDetect(gyroDev_t *gyro) -{ - gyro->init = fakeGyroInit; - gyro->intStatus = fakeGyroInitStatus; - gyro->read = fakeGyroRead; - gyro->temperature = fakeGyroReadTemp; - gyro->scale = 1.0f / 16.4f; - return true; -} -#endif - -#ifdef USE_FAKE_ACC -int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0}; - -static void fakeAccInit(accDev_t *acc) {UNUSED(acc);} - -static bool fakeAccRead(int16_t *accData) { - for(int i=0;iinit = fakeAccInit; - acc->read = fakeAccRead; - acc->acc_1G = 512*8; - acc->revisionCode = 0; - return true; -} -#endif - bool gyroDetect(gyroDev_t *dev) { gyroSensor_e gyroHardware = GYRO_DEFAULT; diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index cef064b7e3..6f1f506bff 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -7,8 +7,8 @@ TARGET_SRC = \ drivers/accgyro_l3gd20.c \ drivers/accgyro_l3g4200d.c \ drivers/accgyro_lsm303dlhc.c \ - drivers/compass_hmc5883l.c \ drivers/accgyro_adxl345.c \ + drivers/accgyro_fake.c \ drivers/accgyro_bma280.c \ drivers/accgyro_mma845x.c \ drivers/accgyro_mpu.c \ @@ -20,9 +20,11 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu9250.c \ drivers/barometer_bmp085.c \ drivers/barometer_bmp280.c \ + drivers/barometer_fake.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8963.c \ drivers/compass_ak8975.c \ + drivers/compass_fake.c \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ drivers/max7456.c From 46a2560237d82b860a7792dbfb2e93ed8d891cc4 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 6 Dec 2016 23:43:35 +1300 Subject: [PATCH 17/20] Make 'MSP_STATUS' compatible to implementation in cleanflight. --- src/main/fc/fc_msp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 4eab72f527..6f25878acc 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -606,6 +606,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); sbufWriteU32(dst, packFlightModeFlags()); sbufWriteU8(dst, masterConfig.current_profile_index); + sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); + sbufWriteU16(dst, 0); // gyro cycle time break; case MSP_RAW_IMU: From 41dda49b440510c4ef6b399e02bfb16ada6da3c8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 6 Dec 2016 13:24:04 +0000 Subject: [PATCH 18/20] Added ability to display PIDs in OSD --- src/main/fc/fc_msp.c | 5 ++++- src/main/io/osd.c | 27 +++++++++++++++++++++++++++ src/main/io/osd.h | 3 +++ src/main/io/serial_cli.c | 33 ++++++++++++++++++--------------- 4 files changed, 52 insertions(+), 16 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 4eab72f527..d545c90cf1 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1559,7 +1559,10 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) osdProfile()->alt_alarm = sbufReadU16(src); } else { // set a position setting - osdProfile()->item_pos[addr] = sbufReadU16(src); + const uint16_t pos = sbufReadU16(src); + if (addr < OSD_ITEM_COUNT) { + osdProfile()->item_pos[addr] = pos; + } } } break; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 92d071de1f..4dbd91cafe 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -333,6 +333,27 @@ static void osdDrawSingleElement(uint8_t item) return; } + case OSD_ROLL_PIDS: + { + const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + sprintf(buff, "ROL %3d,%3d,%3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]); + break; + } + + case OSD_PITCH_PIDS: + { + const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + sprintf(buff, "PIT %3d,%3d,%3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]); + break; + } + + case OSD_YAW_PIDS: + { + const pidProfile_t *pidProfile = ¤tProfile->pidProfile; + sprintf(buff, "YAW %3d,%3d,%3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]); + break; + } + default: return; } @@ -372,6 +393,9 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_MAH_DRAWN); osdDrawSingleElement(OSD_CRAFT_NAME); osdDrawSingleElement(OSD_ALTITUDE); + osdDrawSingleElement(OSD_ROLL_PIDS); + osdDrawSingleElement(OSD_PITCH_PIDS); + osdDrawSingleElement(OSD_YAW_PIDS); #ifdef GPS #ifdef CMS @@ -403,6 +427,9 @@ void osdResetConfig(osd_profile_t *osdProfile) osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); + osdProfile->item_pos[OSD_ROLL_PIDS] = OSD_POS(2, 10) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_PITCH_PIDS] = OSD_POS(2, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_YAW_PIDS] = OSD_POS(2, 12) | VISIBLE_FLAG; osdProfile->rssi_alarm = 20; osdProfile->cap_alarm = 2200; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 5a6a28ed1e..c3642218ad 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -40,6 +40,9 @@ typedef enum { OSD_GPS_SPEED, OSD_GPS_SATS, OSD_ALTITUDE, + OSD_ROLL_PIDS, + OSD_PITCH_PIDS, + OSD_YAW_PIDS, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 607c2ffdb3..159df98af5 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -967,21 +967,24 @@ const clivalue_t valueTable[] = { { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->time_alarm, .config.minmax = { 0, 60 } }, { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->alt_alarm, .config.minmax = { 0, 10000 } }, - { "osd_main_voltage_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, 65536 } }, - { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, 65536 } }, - { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, 65536 } }, - { "osd_ontime_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, 65536 } }, - { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, 65536 } }, - { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, 65536 } }, - { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, 65536 } }, - { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, 65536 } }, - { "osd_artificial_horizon", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, 65536 } }, - { "osd_current_draw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, 65536 } }, - { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, 65536 } }, - { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, 65536 } }, - { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, 65536 } }, - { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, 65536 } }, - { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, 65536 } }, + { "osd_main_voltage_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, UINT16_MAX } }, + { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, UINT16_MAX } }, + { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, UINT16_MAX } }, + { "osd_ontime_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, UINT16_MAX } }, + { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, UINT16_MAX } }, + { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, UINT16_MAX } }, + { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, UINT16_MAX } }, + { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, UINT16_MAX } }, + { "osd_artificial_horizon", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, UINT16_MAX } }, + { "osd_current_draw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, UINT16_MAX } }, + { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, UINT16_MAX } }, + { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, UINT16_MAX } }, + { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, UINT16_MAX } }, + { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, UINT16_MAX } }, + { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, UINT16_MAX } }, + { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, UINT16_MAX } }, + { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, UINT16_MAX } }, + { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, UINT16_MAX } }, #endif #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } }, From 20bf9c6d7a25b0d8f11a0914fd69da0d1a4361b3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 6 Dec 2016 16:34:35 +0000 Subject: [PATCH 19/20] Removed commas from PID OSD display --- src/main/io/osd.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4dbd91cafe..ca21509f26 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -336,21 +336,21 @@ static void osdDrawSingleElement(uint8_t item) case OSD_ROLL_PIDS: { const pidProfile_t *pidProfile = ¤tProfile->pidProfile; - sprintf(buff, "ROL %3d,%3d,%3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]); + sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]); break; } case OSD_PITCH_PIDS: { const pidProfile_t *pidProfile = ¤tProfile->pidProfile; - sprintf(buff, "PIT %3d,%3d,%3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]); + sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]); break; } case OSD_YAW_PIDS: { const pidProfile_t *pidProfile = ¤tProfile->pidProfile; - sprintf(buff, "YAW %3d,%3d,%3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]); + sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]); break; } From d76ce7ff519022c32e182ea2d3b1f3a33c493842 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 7 Dec 2016 13:39:09 +0100 Subject: [PATCH 20/20] cleaned #define USE_DSHOT --- src/main/target/BETAFLIGHTF3/target.h | 1 - src/main/target/BLUEJAYF4/target.h | 1 - src/main/target/DOGE/target.h | 1 - src/main/target/FURYF3/target.h | 1 - src/main/target/FURYF4/target.h | 1 - src/main/target/IMPULSERCF3/target.h | 1 - src/main/target/KISSFC/target.h | 1 - src/main/target/LUX_RACE/target.h | 1 - src/main/target/MOTOLAB/target.h | 1 - src/main/target/MULTIFLITEPICO/target.h | 1 - src/main/target/OMNIBUS/target.h | 1 - src/main/target/OMNIBUSF4/target.h | 1 - src/main/target/RACEBASE/target.h | 2 -- src/main/target/REVO/target.h | 1 - src/main/target/SPARKY2/target.h | 1 - src/main/target/SPRACINGF3/target.h | 1 - src/main/target/STM32F3DISCOVERY/target.h | 1 - src/main/target/TINYFISH/target.h | 2 -- src/main/target/X_RACERSPI/target.h | 1 - src/main/target/YUPIF4/target.h | 1 - 20 files changed, 22 deletions(-) diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index cb581a2f78..f22a23573c 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -44,7 +44,6 @@ #define MPU_INT_EXTI PC13 #define USE_EXTI -#define USE_DSHOT #define USE_ESC_TELEMETRY #define REMAP_TIM16_DMA #define REMAP_TIM17_DMA diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 921861a11c..ad588a23ae 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -144,7 +144,6 @@ #define USE_ADC #define VBAT_ADC_PIN PC3 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define LED_STRIP diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index ed75d987e8..5b360ed8dd 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -125,7 +125,6 @@ #define ENSURE_MPU_DATA_READY_IS_LOW #define LED_STRIP -#define USE_DSHOT #define USE_ESC_TELEMETRY #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 81012826b7..a858e97324 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -161,7 +161,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define USE_DSHOT #define USE_ESC_TELEMETRY #define REMAP_TIM17_DMA diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 1317168a70..790d3096d5 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -175,7 +175,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define USE_DSHOT #define USE_ESC_TELEMETRY #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index ed5475b30e..9689883a5c 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -46,7 +46,6 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define REMAP_TIM17_DMA diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 07dbd7b6e5..2551072af6 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -23,7 +23,6 @@ #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR) -#define USE_DSHOT #define USE_ESC_TELEMETRY #define USE_ESCSERIAL diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 2fe4428e82..fa6eb8a9f1 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -46,7 +46,6 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_DSHOT #define USE_ESC_TELEMETRY #define USE_SPI diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index d2fe6f1df9..ef18dbd7c2 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -95,7 +95,6 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define USE_DSHOT #define USE_ESC_TELEMETRY #define SPEKTRUM_BIND diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index be8f5b285a..e56633eaf3 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/src/main/target/MULTIFLITEPICO/target.h @@ -105,7 +105,6 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define REMAP_TIM17_DMA diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 524b9f33bd..166a570352 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -128,7 +128,6 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 -#define USE_DSHOT #define USE_ESC_TELEMETRY // DSHOT output 4 uses DMA1_Channel5, so don't use it for the SDCARD until we find an alternative diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 673b929bc1..597091dbc3 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -155,7 +155,6 @@ #define VBAT_ADC_PIN PC2 //#define RSSI_ADC_PIN PA0 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define LED_STRIP diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 8e24a1455e..8c78e237dd 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -116,8 +116,6 @@ #define TARGET_IO_PORTC (BIT(5)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USE_DSHOT - #if defined(USE_UART3_RX_DMA) && defined(USE_DSHOT) #undef USE_UART3_RX_DMA #endif diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 23d2f5aa9d..710590d1dd 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -41,7 +41,6 @@ #endif -#define USE_DSHOT #define USE_ESC_TELEMETRY #define LED0 PB5 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 8ca1f6b3bd..5a33df4214 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -35,7 +35,6 @@ #define INVERTER PC6 #define INVERTER_USART USART6 -#define USE_DSHOT #define USE_ESC_TELEMETRY // MPU9250 interrupt diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 5c2ea83426..3d906295e5 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -115,7 +115,6 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define REMAP_TIM17_DMA diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index a5ddc38d02..a43b50fafe 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -169,7 +169,6 @@ #define RSSI_ADC_PIN PC2 #define EXTERNAL1_ADC_PIN PC3 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define LED_STRIP diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h index 86b41936a9..e47e42b23c 100644 --- a/src/main/target/TINYFISH/target.h +++ b/src/main/target/TINYFISH/target.h @@ -106,8 +106,6 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define USE_DSHOT - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_TELEMETRY) #define TARGET_CONFIG diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 72406077d4..8ade290120 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -101,7 +101,6 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define REMAP_TIM17_DMA diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 2387d3544c..40387f8770 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -138,7 +138,6 @@ #define VBAT_ADC_PIN PC1 #define RSSI_ADC_GPIO_PIN PC0 -#define USE_DSHOT #define USE_ESC_TELEMETRY #define LED_STRIP