From 948c66817ea3da62cf35acb07f8838d72de49357 Mon Sep 17 00:00:00 2001 From: ellingo Date: Sat, 18 Feb 2017 19:25:41 +0100 Subject: [PATCH 01/27] Barometer initialization improvements Assuming 1 std atm instead of vacuum before first calibration. Sets altitude to zero during calibration (copied from cleanflight). Stops calibration early when measurements are stable. --- src/main/sensors/barometer.c | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 4b25477dc5..83c34ad5d5 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -51,7 +51,7 @@ static int32_t baroPressure = 0; static int32_t baroTemperature = 0; static int32_t baroGroundAltitude = 0; -static int32_t baroGroundPressure = 0; +static int32_t baroGroundPressure = 8*101325; static uint32_t baroPressureSum = 0; bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) @@ -222,20 +222,31 @@ int32_t baroCalculateAltitude(void) // calculates height from ground via baro readings // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 - BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm - BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise - + if (isBaroCalibrationComplete()) { + BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm + BaroAlt_tmp -= baroGroundAltitude; + baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise + } + else { + baro.BaroAlt = 0; + } return baro.BaroAlt; } void performBaroCalibrationCycle(void) { + static int32_t savedGroundPressure = 0; + baroGroundPressure -= baroGroundPressure / 8; baroGroundPressure += baroPressureSum / PRESSURE_SAMPLE_COUNT; baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; - calibratingB--; + if (baroGroundPressure == savedGroundPressure) + calibratingB = 0; + else { + calibratingB--; + savedGroundPressure=baroGroundPressure; + } } #endif /* BARO */ From 270a7e11d727d8eae1349f4cd0bdf803aa5ac0cd Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 17 Feb 2017 13:57:28 +1300 Subject: [PATCH 02/27] Renamed 'dfu' command to 'bootloader' to make it congruent with STM terminology. --- src/main/fc/cli.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ffaf4c3ba3..0f60993c50 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3124,11 +3124,11 @@ static void cliReboot(void) cliRebootEx(false); } -static void cliDfu(char *cmdLine) +static void cliBootloader(char *cmdLine) { UNUSED(cmdLine); - cliPrintHashLine("restarting in DFU mode"); + cliPrintHashLine("restarting in bootloader mode"); cliRebootEx(true); } @@ -4151,7 +4151,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), + CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|profile|rates|all] {showdefaults}", cliDiff), CLI_COMMAND_DEF("dump", "dump configuration", From 32a6409fc4fe7bd6a13f8157ee0eb306f822b0a0 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 22 Feb 2017 19:43:16 +0100 Subject: [PATCH 03/27] Move EEPROM configuration space to Flash Sector 1 for all F7 targets Add correct flash layout for STM32F722 Remove obsolete CONFIG_START_FLASH_ADDRESS definition --- src/main/config/config_streamer.c | 76 ++++++----- src/main/target/ALIENFLIGHTF4/target.h | 2 - src/main/target/ANYFCF7/target.h | 2 - src/main/target/BLUEJAYF4/target.h | 2 - src/main/target/COLIBRI/target.h | 2 - src/main/target/ELLE0/target.h | 1 - src/main/target/F4BY/target.h | 2 - src/main/target/FISHDRONEF4/target.h | 1 - src/main/target/FURYF4/target.h | 1 - src/main/target/FURYF7/target.h | 2 - src/main/target/KAKUTEF4/target.h | 2 - src/main/target/KIWIF4/target.h | 1 - src/main/target/NERO/target.h | 2 - src/main/target/NUCLEOF7/target.h | 2 - src/main/target/OMNIBUSF4/target.h | 2 - src/main/target/REVO/target.h | 2 - src/main/target/REVONANO/target.h | 2 - src/main/target/SPARKY2/target.h | 2 - src/main/target/VRRACE/target.h | 1 - src/main/target/YUPIF4/target.h | 2 - src/main/target/link/stm32_flash_f7.ld | 162 +++++++++++++++++++++++ src/main/target/link/stm32_flash_f722.ld | 7 +- src/main/target/link/stm32_flash_f745.ld | 7 +- src/main/target/link/stm32_flash_f746.ld | 7 +- 24 files changed, 216 insertions(+), 76 deletions(-) create mode 100644 src/main/target/link/stm32_flash_f7.ld diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index 3473d2a17b..5028390f17 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -26,41 +26,12 @@ extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; -#if !defined(FLASH_PAGE_SIZE) -// F1 -# if defined(STM32F10X_MD) -# define FLASH_PAGE_SIZE (0x400) -# elif defined(STM32F10X_HD) -# define FLASH_PAGE_SIZE (0x800) -// F3 -# elif defined(STM32F303xC) -# define FLASH_PAGE_SIZE (0x800) -// F4 -# elif defined(STM32F40_41xxx) -# define FLASH_PAGE_SIZE ((uint32_t)0x20000) -# elif defined (STM32F411xE) -# define FLASH_PAGE_SIZE ((uint32_t)0x20000) -# elif defined(STM32F427_437xx) -# define FLASH_PAGE_SIZE ((uint32_t)0x20000) // 128K sectors -# elif defined (STM32F446xx) -# define FLASH_PAGE_SIZE ((uint32_t)0x20000) -// F7 -#elif defined(STM32F722xx) -# define FLASH_PAGE_SIZE ((uint32_t)0x20000) -# elif defined(STM32F745xx) -# define FLASH_PAGE_SIZE ((uint32_t)0x40000) -# elif defined(STM32F746xx) -# define FLASH_PAGE_SIZE ((uint32_t)0x40000) -# elif defined(UNIT_TEST) -# define FLASH_PAGE_SIZE (0x400) -# else -# error "Flash page size not defined for target." -# endif -#endif +uint32_t FlashPageSize; void config_streamer_init(config_streamer_t *c) { memset(c, 0, sizeof(*c)); + FlashPageSize = (uint32_t)&__config_end - (uint32_t)&__config_start; } void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) @@ -93,7 +64,7 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) c->err = 0; } -#if defined(STM32F7) +#if defined(STM32F745xx) || defined(STM32F746xx) /* Sector 0 0x08000000 - 0x08007FFF 32 Kbytes Sector 1 0x08008000 - 0x0800FFFF 32 Kbytes @@ -130,6 +101,43 @@ static uint32_t getFLASHSectorForEEPROM(void) } } +#elif defined(STM32F722xx) +/* +Sector 0 0x08000000 - 0x08003FFF 16 Kbytes +Sector 1 0x08004000 - 0x08007FFF 16 Kbytes +Sector 2 0x08008000 - 0x0800BFFF 16 Kbytes +Sector 3 0x0800C000 - 0x0800FFFF 16 Kbytes +Sector 4 0x08010000 - 0x0801FFFF 64 Kbytes +Sector 5 0x08020000 - 0x0803FFFF 128 Kbytes +Sector 6 0x08040000 - 0x0805FFFF 128 Kbytes +Sector 7 0x08060000 - 0x0807FFFF 128 Kbytes +*/ + +static uint32_t getFLASHSectorForEEPROM(void) +{ + if ((uint32_t)&__config_start <= 0x08003FFF) + return FLASH_SECTOR_0; + if ((uint32_t)&__config_start <= 0x08007FFF) + return FLASH_SECTOR_1; + if ((uint32_t)&__config_start <= 0x0800BFFF) + return FLASH_SECTOR_2; + if ((uint32_t)&__config_start <= 0x0800FFFF) + return FLASH_SECTOR_3; + if ((uint32_t)&__config_start <= 0x0801FFFF) + return FLASH_SECTOR_4; + if ((uint32_t)&__config_start <= 0x0803FFFF) + return FLASH_SECTOR_5; + if ((uint32_t)&__config_start <= 0x0805FFFF) + return FLASH_SECTOR_6; + if ((uint32_t)&__config_start <= 0x0807FFFF) + return FLASH_SECTOR_7; + + // Not good + while (1) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } +} + #elif defined(STM32F4) /* Sector 0 0x08000000 - 0x08003FFF 16 Kbytes @@ -186,7 +194,7 @@ static int write_word(config_streamer_t *c, uint32_t value) return c->err; } #if defined(STM32F7) - if (c->address % FLASH_PAGE_SIZE == 0) { + if (c->address % FlashPageSize == 0) { FLASH_EraseInitTypeDef EraseInitStruct = { .TypeErase = FLASH_TYPEERASE_SECTORS, .VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V @@ -204,7 +212,7 @@ static int write_word(config_streamer_t *c, uint32_t value) return -2; } #else - if (c->address % FLASH_PAGE_SIZE == 0) { + if (c->address % FlashPageSize == 0) { #if defined(STM32F4) const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000 #else diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index cecd4bc2c9..9a0a50e15d 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -23,8 +23,6 @@ #define HW_PIN PC13 #define BRUSHED_ESC_AUTODETECT -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "AlienFlight F4" #define LED0 PC12 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 6dd3ecc52d..c74d0af646 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -19,8 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "ANY7" -#define CONFIG_START_FLASH_ADDRESS (0x080C0000) - #define USBD_PRODUCT_STRING "AnyFCF7" #define USE_DSHOT diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 482f3ee7f3..95698f5f9e 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -21,8 +21,6 @@ #define TARGET_VALIDATECONFIG #define TARGET_PREINIT -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "BlueJayF4" #define USE_HARDWARE_REVISION_DETECTION diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 9cf92bd1a4..e46f52d25e 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -19,8 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "COLI" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "Colibri" #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" diff --git a/src/main/target/ELLE0/target.h b/src/main/target/ELLE0/target.h index 4c1f03b779..17c71981e6 100644 --- a/src/main/target/ELLE0/target.h +++ b/src/main/target/ELLE0/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "ELL0" -#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define TARGET_XTAL_MHZ 25 #define USBD_PRODUCT_STRING "Elle0" diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 39199d241f..248e4c3136 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -18,8 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "F4BY" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "Swift-Flyer F4BY" #define LED0 PE3 // Blue LED diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index cf3a9fb132..4b936f28cc 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FDF4" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define USBD_PRODUCT_STRING "FishDroneF4" diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index ae67882d9d..7ad82b6a68 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FYF4" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define USBD_PRODUCT_STRING "FuryF4" diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index ff108c7c8d..557227272f 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -19,8 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "FYF7" -#define CONFIG_START_FLASH_ADDRESS (0x080C0000) - #define USBD_PRODUCT_STRING "FuryF7" #define USE_DSHOT diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8c4deb0e6a..7089dda684 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -18,8 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "KTV1" -#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "KakuteF4-V1" #define LED0 PB5 diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 2f5b4af279..10ad0eb0bf 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "KIWI" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define USBD_PRODUCT_STRING "KIWIF4" diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index 85aa854b13..3817147f3b 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -18,8 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "NERO" -#define CONFIG_START_FLASH_ADDRESS (0x08060000) - #define USBD_PRODUCT_STRING "NERO" #define HW_PIN PB2 diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index 3d4a472b84..07f0789262 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -19,8 +19,6 @@ #define TARGET_BOARD_IDENTIFIER "NUC7" -#define CONFIG_START_FLASH_ADDRESS (0x080C0000) - #define USBD_PRODUCT_STRING "NucleoF7" //#define USE_DSHOT diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 1e55afd7f3..ca263ec44b 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -21,8 +21,6 @@ #define TARGET_BOARD_IDENTIFIER "OBF4" #endif -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "OmnibusF4" #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 9c7c0c01a2..2c51c649b7 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -17,8 +17,6 @@ #pragma once -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #if defined(AIRBOTF4) #define TARGET_BOARD_IDENTIFIER "AIR4" #define USBD_PRODUCT_STRING "AirbotF4" diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 998f7ac0c2..74114e633b 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -18,8 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "REVN" -#define CONFIG_START_FLASH_ADDRESS (0x08060000) //0x08060000 to 0x08080000 (FLASH_Sector_7) - #define USBD_PRODUCT_STRING "Revo Nano" #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8010000" diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index a516a8f469..7d9ba97e6d 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -18,8 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "SPK2" -#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "Sparky 2.0" #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index 89aa9f696f..0e78778f3b 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -18,7 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "VRRA" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define USBD_PRODUCT_STRING "VRRACE" diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 461af8aefd..1c3102be2c 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -18,8 +18,6 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "YPF4" -#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) - #define USBD_PRODUCT_STRING "YupiF4" #define LED0 PB6 diff --git a/src/main/target/link/stm32_flash_f7.ld b/src/main/target/link/stm32_flash_f7.ld new file mode 100644 index 0000000000..bd9a67b7b2 --- /dev/null +++ b/src/main/target/link/stm32_flash_f7.ld @@ -0,0 +1,162 @@ +/* +***************************************************************************** +** +** File : stm32_flash_f7.ld +** +** Abstract : Common linker script for STM32 devices. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM); /* end of RAM */ + +/* Base address where the config is stored. */ +__config_start = ORIGIN(FLASH_CONFIG); +__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + PROVIDE (isr_vector_table_base = .); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH1 + + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = .; + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */ + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >STACKRAM = 0xa5 + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld index 3e58617806..1e0118dfd1 100644 --- a/src/main/target/link/stm32_flash_f722.ld +++ b/src/main/target/link/stm32_flash_f722.ld @@ -15,8 +15,9 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 384K - FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K @@ -26,4 +27,4 @@ MEMORY /* note TCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_f7.ld" diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index c8da9a3aa2..7669df69be 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -15,8 +15,9 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 768K - FLASH_CONFIG (r) : ORIGIN = 0x080C0000, LENGTH = 256K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K + FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K + FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K @@ -25,4 +26,4 @@ MEMORY /* note CCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_f7.ld" diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld index 1b92f2ba33..b386cf8ae8 100644 --- a/src/main/target/link/stm32_flash_f746.ld +++ b/src/main/target/link/stm32_flash_f746.ld @@ -15,8 +15,9 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 768K - FLASH_CONFIG (r) : ORIGIN = 0x080C0000, LENGTH = 256K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K + FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K + FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K @@ -25,4 +26,4 @@ MEMORY /* note CCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_f7.ld" From 244a2f811f4fbd8c13ba222ef00c63a02cbe42e1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 24 Feb 2017 19:13:51 +0100 Subject: [PATCH 04/27] Move EEPROM configuration space for F4 targets Some cean-up --- src/main/target/link/stm32_flash_f405.ld | 14 +- src/main/target/link/stm32_flash_f405_opbl.ld | 16 ++- src/main/target/link/stm32_flash_f411.ld | 14 +- src/main/target/link/stm32_flash_f411_opbl.ld | 16 ++- src/main/target/link/stm32_flash_f446.ld | 14 +- src/main/target/link/stm32_flash_f4xx.ld | 134 ------------------ src/main/target/link/stm32_flash_f722.ld | 9 +- src/main/target/link/stm32_flash_f745.ld | 9 +- src/main/target/link/stm32_flash_f746.ld | 9 +- ...stm32_flash_f7.ld => stm32_flash_split.ld} | 2 +- 10 files changed, 67 insertions(+), 170 deletions(-) delete mode 100644 src/main/target/link/stm32_flash_f4xx.ld rename src/main/target/link/{stm32_flash_f7.ld => stm32_flash_split.ld} (99%) diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld index a68b4c113a..d7ba9ca86d 100644 --- a/src/main/target/link/stm32_flash_f405.ld +++ b/src/main/target/link/stm32_flash_f405.ld @@ -13,16 +13,18 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x080DFFFF 896K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 +0x08000000 to 0x080FFFFF 1024K full flash, +0x08000000 to 0x08003FFF 16K isr vector, startup code, +0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 +0x08008000 to 0x080FFFFF 992K firmware, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 896K - FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 992K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K @@ -32,4 +34,4 @@ MEMORY REGION_ALIAS("STACKRAM", CCM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_split.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld index 667d83b979..a78cf601f9 100644 --- a/src/main/target/link/stm32_flash_f405_opbl.ld +++ b/src/main/target/link/stm32_flash_f405_opbl.ld @@ -13,17 +13,19 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x08004000 16K OPBL, -0x08004000 to 0x080DFFFF 880K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 +0x08000000 to 0x080FFFFF 1024K full flash, +0x08000000 to 0x08003FFF 16K OPBL, +0x08004000 to 0x08007FFF 16K isr vector, startup code, +0x08008000 to 0x0800BFFF 16K config, // FLASH_Sector_2 +0x0800C000 to 0x080FFFFF 976K firmware, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 880K - FLASH_CONFIG (r): ORIGIN = 0x080E0000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH_CONFIG (r): ORIGIN = 0x08008000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x0800C000, LENGTH = 976K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K @@ -32,4 +34,4 @@ MEMORY REGION_ALIAS("STACKRAM", CCM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld index 1e8bf58c9d..18684c2593 100644 --- a/src/main/target/link/stm32_flash_f411.ld +++ b/src/main/target/link/stm32_flash_f411.ld @@ -13,16 +13,18 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08080000 512K full flash, -0x08000000 to 0x0805FFFF 384K firmware, -0x08060000 to 0x08080000 128K config, +0x08000000 to 0x0807FFFF 512K full flash, +0x08000000 to 0x08003FFF 16K isr vector, startup code, +0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 +0x08008000 to 0x0807FFFF 480K firmware, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 384K - FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K @@ -30,4 +32,4 @@ MEMORY REGION_ALIAS("STACKRAM", RAM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld index f8e23b4b44..4a57959110 100644 --- a/src/main/target/link/stm32_flash_f411_opbl.ld +++ b/src/main/target/link/stm32_flash_f411_opbl.ld @@ -13,17 +13,19 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08080000 512K full flash, -0x08000000 to 0x08004000 16K OPBL, -0x08004000 to 0x0805FFFF 368K firmware, -0x08060000 to 0x08080000 128K config, +0x08000000 to 0x0807FFFF 512K full flash, +0x08000000 to 0x08003FFF 16K OPBL, +0x08004000 to 0x08007FFF 16K isr vector, startup code, +0x08008000 to 0x0800BFFF 16K config, // FLASH_Sector_2 +0x0800C000 to 0x0807FFFF 464K firmware, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 368K - FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x0800C000, LENGTH = 464K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K @@ -32,4 +34,4 @@ MEMORY REGION_ALIAS("STACKRAM", CCM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f446.ld b/src/main/target/link/stm32_flash_f446.ld index d875a0ebc1..a258aa59dc 100644 --- a/src/main/target/link/stm32_flash_f446.ld +++ b/src/main/target/link/stm32_flash_f446.ld @@ -13,16 +13,18 @@ ENTRY(Reset_Handler) /* -0x08000000 to 0x08080000 512K full flash, -0x08000000 to 0x0805FFFF 384K firmware, -0x08060000 to 0x08080000 128K config, +0x08000000 to 0x0807FFFF 512K full flash, +0x08000000 to 0x08003FFF 16K isr vector, startup code, +0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 +0x08008000 to 0x0807FFFF 480K firmware, */ /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 384K - FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K @@ -30,4 +32,4 @@ MEMORY REGION_ALIAS("STACKRAM", RAM) -INCLUDE "stm32_flash.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f4xx.ld b/src/main/target/link/stm32_flash_f4xx.ld deleted file mode 100644 index dcb0147d27..0000000000 --- a/src/main/target/link/stm32_flash_f4xx.ld +++ /dev/null @@ -1,134 +0,0 @@ - -/* Internal Memory Map*/ -MEMORY -{ - rom (rx) : ORIGIN = 0x08000000, LENGTH = 0x00100000 - ram (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - ram1 (rwx) : ORIGIN = 0x10000000, LENGTH = 0x00010000 -} - -_eram = 0x20000000 + 0x00020000; -SECTIONS -{ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) - *(.text*) - - KEEP(*(.init)) - KEEP(*(.fini)) - - /* .ctors */ - *crtbegin.o(.ctors) - *crtbegin?.o(.ctors) - *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors) - *(SORT(.ctors.*)) - *(.ctors) - - /* .dtors */ - *crtbegin.o(.dtors) - *crtbegin?.o(.dtors) - *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors) - *(SORT(.dtors.*)) - *(.dtors) - - *(.rodata*) - - KEEP(*(.eh_fram e*)) - } > rom - - .ARM.extab : - { - *(.ARM.extab* .gnu.linkonce.armextab.*) - } > rom - - __exidx_start = .; - .ARM.exidx : - { - *(.ARM.exidx* .gnu.linkonce.armexidx.*) - } > rom - __exidx_end = .; - __etext = .; - - /* _sidata is used in coide startup code */ - _sidata = __etext; - - .data : AT (__etext) - { - __data_start__ = .; - - /* _sdata is used in coide startup code */ - _sdata = __data_start__; - - *(vtable) - *(.data*) - - . = ALIGN(4); - /* preinit data */ - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP(*(.preinit_array)) - PROVIDE_HIDDEN (__preinit_array_end = .); - - . = ALIGN(4); - /* init data */ - PROVIDE_HIDDEN (__init_array_start = .); - KEEP(*(SORT(.init_array.*))) - KEEP(*(.init_array)) - PROVIDE_HIDDEN (__init_array_end = .); - - . = ALIGN(4); - /* finit data */ - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP(*(SORT(.fini_array.*))) - KEEP(*(.fini_array)) - PROVIDE_HIDDEN (__fini_array_end = .); - - KEEP(*(.jcr*)) - . = ALIGN(4); - /* All data end */ - __data_end__ = .; - - /* _edata is used in coide startup code */ - _edata = __data_end__; - } > ram - - .bss : - { - . = ALIGN(4); - __bss_start__ = .; - _sbss = __bss_start__; - *(.bss*) - *(COMMON) - . = ALIGN(4); - __bss_end__ = .; - _ebss = __bss_end__; - } > ram - - .heap (COPY): - { - __end__ = .; - _end = __end__; - end = __end__; - *(.heap*) - __HeapLimit = .; - } > ram - - /* .stack_dummy section doesn't contains any symbols. It is only - * used for linker to calculate size of stack sections, and assign - * values to stack symbols later */ - .co_stack (NOLOAD): - { - . = ALIGN(8); - *(.co_stack .co_stack.*) - } > ram - - /* Set stack top to end of ram , and stack limit move down by - * size of stack_dummy section */ - __StackTop = ORIGIN(ram ) + LENGTH(ram ); - __StackLimit = __StackTop - SIZEOF(.co_stack); - PROVIDE(__stack = __StackTop); - - /* Check if data + heap + stack exceeds ram limit */ - ASSERT(__StackLimit >= __HeapLimit, "region ram overflowed with stack") -} \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld index 1e0118dfd1..f3b0e5b67c 100644 --- a/src/main/target/link/stm32_flash_f722.ld +++ b/src/main/target/link/stm32_flash_f722.ld @@ -12,6 +12,13 @@ /* Entry Point */ ENTRY(Reset_Handler) +/* +0x08000000 to 0x0807FFFF 512K full flash, +0x08000000 to 0x08003FFF 16K isr vector, startup code, +0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 +0x08008000 to 0x0807FFFF 480K firmware, +*/ + /* Specify the memory areas */ MEMORY { @@ -27,4 +34,4 @@ MEMORY /* note TCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) -INCLUDE "stm32_flash_f7.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index 7669df69be..306d669b44 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -12,6 +12,13 @@ /* Entry Point */ ENTRY(Reset_Handler) +/* +0x08000000 to 0x080FFFFF 1024K full flash, +0x08000000 to 0x08007FFF 32K isr vector, startup code, +0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 +0x08010000 to 0x080FFFFF 960K firmware, +*/ + /* Specify the memory areas */ MEMORY { @@ -26,4 +33,4 @@ MEMORY /* note CCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) -INCLUDE "stm32_flash_f7.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld index b386cf8ae8..16be8311d8 100644 --- a/src/main/target/link/stm32_flash_f746.ld +++ b/src/main/target/link/stm32_flash_f746.ld @@ -12,6 +12,13 @@ /* Entry Point */ ENTRY(Reset_Handler) +/* +0x08000000 to 0x080FFFFF 1024K full flash, +0x08000000 to 0x08007FFF 32K isr vector, startup code, +0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 +0x08010000 to 0x080FFFFF 960K firmware, +*/ + /* Specify the memory areas */ MEMORY { @@ -26,4 +33,4 @@ MEMORY /* note CCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) -INCLUDE "stm32_flash_f7.ld" +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f7.ld b/src/main/target/link/stm32_flash_split.ld similarity index 99% rename from src/main/target/link/stm32_flash_f7.ld rename to src/main/target/link/stm32_flash_split.ld index bd9a67b7b2..cc6d264fab 100644 --- a/src/main/target/link/stm32_flash_f7.ld +++ b/src/main/target/link/stm32_flash_split.ld @@ -1,7 +1,7 @@ /* ***************************************************************************** ** -** File : stm32_flash_f7.ld +** File : stm32_flash_split.ld ** ** Abstract : Common linker script for STM32 devices. ** From 051c9cca488763c61b0caca437a89752a1c40970 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 24 Feb 2017 22:18:26 +0000 Subject: [PATCH 05/27] Moved rate profiles out of pid profiles --- src/main/blackbox/blackbox.c | 2 +- src/main/cms/cms_menu_blackbox.c | 2 ++ src/main/cms/cms_menu_imu.c | 8 +++---- src/main/cms/cms_menu_ledstrip.c | 11 +++++---- src/main/cms/cms_menu_misc.c | 2 ++ src/main/config/config_master.h | 5 +++- src/main/config/config_profile.h | 4 ---- src/main/fc/cli.c | 32 ++++++++++++------------- src/main/fc/config.c | 27 ++++++++------------- src/main/fc/config.h | 1 + src/main/flight/pid.h | 1 + src/main/io/osd.c | 2 +- src/main/target/COLIBRI/config.c | 4 ++-- src/main/target/COLIBRI_RACE/config.c | 6 ++--- src/main/target/MULTIFLITEPICO/config.c | 2 +- src/main/target/NAZE/config.c | 12 +++++----- 16 files changed, 61 insertions(+), 60 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4705b15d60..7b269deb55 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1179,7 +1179,7 @@ static bool blackboxWriteSysinfo() } const profile_t *currentProfile = &masterConfig.profile[systemConfig()->current_profile_index]; - const controlRateConfig_t *currentControlRateProfile = ¤tProfile->controlRateProfile[masterConfig.profile[systemConfig()->current_profile_index].activeRateProfile]; + const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); switch (xmitState.headerIndex) { BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 2301a1937c..870f5d2e3b 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -47,6 +47,8 @@ #include "drivers/system.h" +#include "fc/config.h" + #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/beeper.h" diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 047f8e8640..5c876d9592 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -67,7 +67,7 @@ static long cmsx_menuImu_onEnter(void) profileIndex = systemConfig()->current_profile_index; tmpProfileIndex = profileIndex + 1; - rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + rateProfileIndex = systemConfig()->activeRateProfile; tmpRateProfileIndex = rateProfileIndex + 1; return 0; @@ -78,7 +78,7 @@ static long cmsx_menuImu_onExit(const OSD_Entry *self) UNUSED(self); systemConfigMutable()->current_profile_index = profileIndex; - masterConfig.profile[profileIndex].activeRateProfile = rateProfileIndex; + systemConfigMutable()->activeRateProfile = rateProfileIndex; return 0; } @@ -174,7 +174,7 @@ static CMS_Menu cmsx_menuPid = { static long cmsx_RateProfileRead(void) { - memcpy(&rateProfile, &masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], sizeof(controlRateConfig_t)); + memcpy(&rateProfile, controlRateProfiles(rateProfileIndex), sizeof(controlRateConfig_t)); return 0; } @@ -183,7 +183,7 @@ static long cmsx_RateProfileWriteback(const OSD_Entry *self) { UNUSED(self); - memcpy(&masterConfig.profile[profileIndex].controlRateProfile[rateProfileIndex], &rateProfile, sizeof(controlRateConfig_t)); + memcpy(controlRateProfilesMutable(rateProfileIndex), &rateProfile, sizeof(controlRateConfig_t)); return 0; } diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index e92f42c99a..fb6a38ec87 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -26,16 +26,19 @@ #include "build/version.h" -#include "drivers/system.h" +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_ledstrip.h" #include "config/config_profile.h" #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "cms/cms.h" -#include "cms/cms_types.h" -#include "cms/cms_menu_ledstrip.h" +#include "drivers/system.h" + +#include "fc/config.h" + #ifdef LED_STRIP diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index c927aa66b1..3fd009b3f5 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -39,6 +39,8 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "fc/rc_controls.h" + #include "flight/mixer.h" #include "rx/rx.h" diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 3d4156a438..0ff5b7670e 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -37,6 +37,7 @@ #include "drivers/display.h" #include "drivers/serial.h" +#include "fc/config.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/fc_core.h" @@ -123,7 +124,6 @@ #define vtxConfig(x) (&masterConfig.vtxConfig) #define beeperConfig(x) (&masterConfig.beeperConfig) - #define featureConfigMutable(x) (&masterConfig.featureConfig) #define systemConfigMutable(x) (&masterConfig.systemConfig) #define motorConfigMutable(x) (&masterConfig.motorConfig) @@ -179,12 +179,14 @@ #define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x]) #define osdConfig(x) (&masterConfig.osdProfile) #define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) +#define controlRateProfiles(x) (&masterConfig.controlRateProfile[x]) #define servoParamsMutable(x) (&servoProfile()->servoConf[x]) #define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[x]) #define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x]) #define osdConfigMutable(x) (&masterConfig.osdProfile) #define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) +#define controlRateProfilesMutable(x) (&masterConfig.controlRateProfile[x]) #endif // System-wide @@ -301,6 +303,7 @@ typedef struct master_s { #endif profile_t profile[MAX_PROFILE_COUNT]; + controlRateConfig_t controlRateProfile[MAX_RATEPROFILES]; modeActivationProfile_t modeActivationProfile; adjustmentProfile_t adjustmentProfile; diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index d1f2c513fd..c741b2a557 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -17,12 +17,8 @@ #pragma once -#include "fc/config.h" -#include "fc/rc_controls.h" #include "flight/pid.h" typedef struct profile_s { pidProfile_t pidProfile; - uint8_t activeRateProfile; - controlRateConfig_t controlRateProfile[MAX_RATEPROFILES]; } profile_t; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c10accf3ca..35c847d7ef 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -725,17 +725,17 @@ static const clivalue_t valueTable[] = { { "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &channelForwardingConfig()->startChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, - { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, - { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, - { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, - { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, - { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, - { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, - { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, + { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, + { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, + { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, + { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, + { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, + { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, + { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, &rxConfig()->airModeActivateThreshold, .config.minmax = {1000, 2000 } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_delay, .config.minmax = { 0, 200 } }, @@ -4127,12 +4127,12 @@ static void printConfig(char *cmdline, bool doDiff) dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { - uint8_t activeProfile = systemConfig()->current_profile_index; + const uint8_t activeProfile = systemConfig()->current_profile_index; for (uint32_t profileCount=0; profileCountactiveRateProfile; - for (uint32_t rateCount = 0; rateCountactiveRateProfile; + for (uint32_t rateCount = 0; rateCount < MAX_RATEPROFILES; rateCount++) { cliDumpRateProfile(rateCount, dumpMask, &defaultConfig); } @@ -4149,7 +4149,7 @@ static void printConfig(char *cmdline, bool doDiff) cliPrint("save"); } else { cliDumpProfile(systemConfig()->current_profile_index, dumpMask, &defaultConfig); - cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); + cliDumpRateProfile(systemConfig()->activeRateProfile, dumpMask, &defaultConfig); } } @@ -4158,7 +4158,7 @@ static void printConfig(char *cmdline, bool doDiff) } if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); + cliDumpRateProfile(systemConfig()->activeRateProfile, dumpMask, &defaultConfig); } #ifdef USE_PARAMETER_GROUPS // restore configs from copies diff --git a/src/main/fc/config.c b/src/main/fc/config.c index ed6eda18e4..41440c8895 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -131,7 +131,7 @@ static void resetCompassConfig(compassConfig_t* compassConfig) } #endif -static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) +static void resetControlRateProfile(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; controlRateConfig->rcYawRate8 = 100; @@ -201,12 +201,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) void resetProfile(profile_t *profile) { resetPidProfile(&profile->pidProfile); - - for (int rI = 0; rIcontrolRateProfile[rI]); - } - - profile->activeRateProfile = 0; } #ifdef GPS @@ -730,8 +724,6 @@ uint8_t getCurrentProfile(void) static void setProfile(uint8_t profileIndex) { currentProfile = &masterConfig.profile[profileIndex]; - currentControlRateProfileIndex = currentProfile->activeRateProfile; - currentControlRateProfile = ¤tProfile->controlRateProfile[currentControlRateProfileIndex]; } uint8_t getCurrentControlRateProfile(void) @@ -739,16 +731,16 @@ uint8_t getCurrentControlRateProfile(void) return currentControlRateProfileIndex; } -static void setControlRateProfile(uint8_t profileIndex) +static void setControlRateProfile(uint8_t controlRateProfileIndex) { - currentControlRateProfileIndex = profileIndex; - masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex; - currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex]; + systemConfigMutable()->activeRateProfile = controlRateProfileIndex; + currentControlRateProfileIndex = controlRateProfileIndex; + currentControlRateProfile = controlRateProfilesMutable(controlRateProfileIndex); } -controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) +const controlRateConfig_t *getControlRateConfig(uint8_t controlRateProfileIndex) { - return &masterConfig.profile[profileIndex].controlRateProfile[masterConfig.profile[profileIndex].activeRateProfile]; + return controlRateProfiles(controlRateProfileIndex); } uint16_t getCurrentMinthrottle(void) @@ -948,6 +940,7 @@ void createDefaultConfig(master_t *config) resetSerialConfig(&config->serialConfig); resetProfile(&config->profile[0]); + resetControlRateProfile(&config->controlRateProfile[0]); config->compassConfig.mag_declination = 0; @@ -1267,7 +1260,7 @@ void readEEPROM(void) // setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); if (systemConfig()->current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check - systemConfig()->current_profile_index = 0; + systemConfigMutable()->current_profile_index = 0; } setProfile(systemConfig()->current_profile_index); @@ -1313,7 +1306,7 @@ void changeProfile(uint8_t profileIndex) if (profileIndex >= MAX_PROFILE_COUNT) { profileIndex = MAX_PROFILE_COUNT - 1; } - systemConfig()->current_profile_index = profileIndex; + systemConfigMutable()->current_profile_index = profileIndex; writeEEPROM(); readEEPROM(); beeperConfirmationBeeps(profileIndex + 1); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index be4e0ad0b8..6303b4a7c2 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -71,6 +71,7 @@ typedef enum { typedef struct systemConfig_s { uint8_t current_profile_index; + uint8_t activeRateProfile; uint8_t debug_mode; uint8_t task_statistics; char name[MAX_NAME_LENGTH + 1]; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c6fd9035ae..dcc24be330 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -18,6 +18,7 @@ #pragma once #include +#include "config/parameter_group.h" #define MAX_PID_PROCESS_DENOM 16 #define PID_CONTROLLER_BETAFLIGHT 1 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fc50e743bb..b2a07f7c2f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -394,7 +394,7 @@ static void osdDrawSingleElement(uint8_t item) case OSD_PIDRATE_PROFILE: { const uint8_t profileIndex = systemConfig()->current_profile_index; - const uint8_t rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + const uint8_t rateProfileIndex = systemConfig()->activeRateProfile; sprintf(buff, "%d-%d", profileIndex + 1, rateProfileIndex + 1); break; } diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 76e4c52101..061f37f3b6 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -50,8 +50,8 @@ void targetConfiguration(master_t *config) //config->rcControlsConfig.yaw_deadband = 10; config->compassConfig.mag_hardware = 1; - config->profile[0].controlRateProfile[0].dynThrPID = 45; - config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700; + config->controlRateProfile[0].dynThrPID = 45; + config->controlRateProfile[0].tpa_breakpoint = 1700; config->serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } #endif diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index 2ae165a846..d79a4fa9ac 100644 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -95,9 +95,9 @@ void targetConfiguration(master_t *config) config->profile[0].pidProfile.I8[YAW] = 50; config->profile[0].pidProfile.D8[YAW] = 20; - config->profile[0].controlRateProfile[0].rates[FD_ROLL] = 86; - config->profile[0].controlRateProfile[0].rates[FD_PITCH] = 86; - config->profile[0].controlRateProfile[0].rates[FD_YAW] = 80; + config->controlRateProfile[0].rates[FD_ROLL] = 86; + config->controlRateProfile[0].rates[FD_PITCH] = 86; + config->controlRateProfile[0].rates[FD_YAW] = 80; targetApplyDefaultLedStripConfig(config->ledStripConfig.ledConfigs); } diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 6c8e8c595c..56656f9d87 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -83,7 +83,7 @@ void targetConfiguration(master_t *config) config->profile[0].pidProfile.I8[PITCH] = 62; config->profile[0].pidProfile.D8[PITCH] = 19; - config->profile[0].controlRateProfile[0].rcRate8 = 70; + config->controlRateProfile[0].rcRate8 = 70; config->profile[0].pidProfile.I8[PIDLEVEL] = 40; } #endif diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 7abbabf3b4..739938b866 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -73,12 +73,12 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.D8[PIDLEVEL] = 30; for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { - config->profile[profileId].controlRateProfile[rateProfileId].rcRate8 = 100; - config->profile[profileId].controlRateProfile[rateProfileId].rcYawRate8 = 110; - config->profile[profileId].controlRateProfile[rateProfileId].rcExpo8 = 0; - config->profile[profileId].controlRateProfile[rateProfileId].rates[ROLL] = 77; - config->profile[profileId].controlRateProfile[rateProfileId].rates[PITCH] = 77; - config->profile[profileId].controlRateProfile[rateProfileId].rates[YAW] = 80; + config->controlRateProfile[rateProfileId].rcRate8 = 100; + config->controlRateProfile[rateProfileId].rcYawRate8 = 110; + config->controlRateProfile[rateProfileId].rcExpo8 = 0; + config->controlRateProfile[rateProfileId].rates[ROLL] = 77; + config->controlRateProfile[rateProfileId].rates[PITCH] = 77; + config->controlRateProfile[rateProfileId].rates[YAW] = 80; config->profile[profileId].pidProfile.dtermSetpointWeight = 200; config->profile[profileId].pidProfile.setpointRelaxRatio = 50; From 4b799ada5e851534bbfa5300e69fa124fe697c13 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 25 Feb 2017 16:24:01 +1100 Subject: [PATCH 06/27] Protection to ensure only one or the other is initialised in regards to blackbox (for BlueJayF4) --- src/main/blackbox/blackbox.c | 3 ++- src/main/blackbox/blackbox.h | 6 ++++++ src/main/fc/fc_init.c | 9 +++++---- src/main/target/BLUEJAYF4/initialisation.c | 10 +++++++--- 4 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4705b15d60..4d14dc587b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1657,7 +1657,8 @@ void handleBlackbox(timeUs_t currentTimeUs) static bool canUseBlackboxWithCurrentConfiguration(void) { - return feature(FEATURE_BLACKBOX); + return feature(FEATURE_BLACKBOX) && + (blackboxConfig()->device != BLACKBOX_SDCARD || feature(FEATURE_SDCARD)); } /** diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index d085f531c4..2d374c673c 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -23,6 +23,12 @@ #include "config/parameter_group.h" +typedef enum { + BLACKBOX_SERIAL = 0, + BLACKBOX_SPIFLASH, + BLACKBOX_SDCARD +} blackBoxDevice_e; + typedef struct blackboxConfig_s { uint8_t rate_num; uint8_t rate_denom; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 292bb89975..4f517afc49 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -488,15 +488,16 @@ void init(void) #endif #ifdef USE_FLASHFS + if (blackboxConfig()->device == BLACKBOX_SPIFLASH) { #if defined(USE_FLASH_M25P16) - m25p16_init(flashConfig()); + m25p16_init(flashConfig()); #endif - - flashfsInit(); + flashfsInit(); + } #endif #ifdef USE_SDCARD - if (feature(FEATURE_SDCARD)) { + if (feature(FEATURE_SDCARD) && blackboxConfig()->device == BLACKBOX_SDCARD) { sdcardInsertionDetectInit(); sdcard_init(sdcardConfig()->useDma); afatfs_init(); diff --git a/src/main/target/BLUEJAYF4/initialisation.c b/src/main/target/BLUEJAYF4/initialisation.c index 312c4511f5..5226d475e5 100644 --- a/src/main/target/BLUEJAYF4/initialisation.c +++ b/src/main/target/BLUEJAYF4/initialisation.c @@ -57,9 +57,13 @@ void targetPreInit(void) /* ensure the CS pin for the flash is pulled hi so any SD card initialisation does not impact the chip */ if (hardwareRevision == BJF4_REV3) { - IO_t io = IOGetByTag(IO_TAG(M25P16_CS_PIN)); - IOConfigGPIO(io, IOCFG_OUT_PP); - IOHi(io); + IO_t flashIo = IOGetByTag(IO_TAG(M25P16_CS_PIN)); + IOConfigGPIO(flashIo, IOCFG_OUT_PP); + IOHi(flashIo); + + IO_t sdcardIo = IOGetByTag(IO_TAG(SDCARD_SPI_CS_PIN)); + IOConfigGPIO(sdcardIo, IOCFG_OUT_PP); + IOHi(sdcardIo); } } From a3ad97b0a4acff8c3e591840af0c0dc667101175 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 25 Feb 2017 07:37:53 +0000 Subject: [PATCH 07/27] Added profile macros and index functions --- src/main/cms/cms_menu_imu.c | 18 ++-- src/main/config/config_master.h | 22 ++-- src/main/fc/cli.c | 134 ++++++++++++------------- src/main/fc/config.c | 27 +++-- src/main/fc/config.h | 7 +- src/main/fc/fc_msp.c | 6 +- src/main/fc/rc_adjustments.c | 2 +- src/main/io/dashboard.c | 33 +++--- src/main/io/osd.c | 4 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- 10 files changed, 123 insertions(+), 132 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 5c876d9592..534cac81b1 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -64,7 +64,7 @@ static controlRateConfig_t rateProfile; static long cmsx_menuImu_onEnter(void) { - profileIndex = systemConfig()->current_profile_index; + profileIndex = getCurrentProfileIndex(); tmpProfileIndex = profileIndex + 1; rateProfileIndex = systemConfig()->activeRateProfile; @@ -77,8 +77,8 @@ static long cmsx_menuImu_onExit(const OSD_Entry *self) { UNUSED(self); - systemConfigMutable()->current_profile_index = profileIndex; - systemConfigMutable()->activeRateProfile = rateProfileIndex; + changeProfile(profileIndex); + changeControlRateProfile(rateProfileIndex); return 0; } @@ -106,7 +106,7 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void static long cmsx_PidRead(void) { - const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + const pidProfile_t *pidProfile = pidProfiles(profileIndex); for (uint8_t i = 0; i < 3; i++) { tempPid[i][0] = pidProfile->P8[i]; tempPid[i][1] = pidProfile->I8[i]; @@ -128,7 +128,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); - pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile_t *pidProfile = pidProfilesMutable(profileIndex); for (uint8_t i = 0; i < 3; i++) { pidProfile->P8[i] = tempPid[i][0]; pidProfile->I8[i] = tempPid[i][1]; @@ -237,7 +237,7 @@ static long cmsx_profileOtherOnEnter(void) { profileIndexString[1] = '0' + tmpProfileIndex; - const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + const pidProfile_t *pidProfile = pidProfiles(profileIndex); cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight; cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio; @@ -252,7 +252,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) { UNUSED(self); - pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile_t *pidProfile = pidProfilesMutable(profileIndex); pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio; pidInitConfig(¤tProfile->pidProfile); @@ -347,7 +347,7 @@ static uint16_t cmsx_yaw_p_limit; static long cmsx_FilterPerProfileRead(void) { - const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + const pidProfile_t *pidProfile = pidProfiles(profileIndex); cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz; cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; @@ -361,7 +361,7 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) { UNUSED(self); - pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile_t *pidProfile = pidProfilesMutable(profileIndex); pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz; pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 0ff5b7670e..2d953bb48c 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -174,19 +174,21 @@ #define vtxConfigMutable(x) (&masterConfig.vtxConfig) #define beeperConfigMutable(x) (&masterConfig.beeperConfig) -#define servoParams(x) (&servoProfile()->servoConf[x]) -#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x]) -#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x]) #define osdConfig(x) (&masterConfig.osdProfile) -#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) -#define controlRateProfiles(x) (&masterConfig.controlRateProfile[x]) +#define servoParams(i) (&servoProfile()->servoConf[i]) +#define adjustmentRanges(i) (&adjustmentProfile()->adjustmentRanges[i]) +#define rxFailsafeChannelConfigs(i) (&masterConfig.rxConfig.failsafe_channel_configurations[i]) +#define modeActivationConditions(i) (&masterConfig.modeActivationProfile.modeActivationConditions[i]) +#define controlRateProfiles(i) (&masterConfig.controlRateProfile[i]) +#define pidProfiles(i) (&masterConfig.profile[i].pidProfile) -#define servoParamsMutable(x) (&servoProfile()->servoConf[x]) -#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[x]) -#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x]) #define osdConfigMutable(x) (&masterConfig.osdProfile) -#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) -#define controlRateProfilesMutable(x) (&masterConfig.controlRateProfile[x]) +#define servoParamsMutable(i) (&servoProfile()->servoConf[i]) +#define adjustmentRangesMutable(i) (&masterConfig.adjustmentProfile.adjustmentRanges[i]) +#define rxFailsafeChannelConfigsMutable(i) (&masterConfig.rxConfig.>failsafe_channel_configurations[i]) +#define modeActivationConditionsMutable(i) (&masterConfig.modeActivationProfile.modeActivationConditions[i]) +#define controlRateProfilesMutable(i) (&masterConfig.controlRateProfile[i]) +#define pidProfilesMutable(i) (&masterConfig.profile[i].pidProfile) #endif // System-wide diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 35c847d7ef..dcc08c6eb8 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -612,15 +612,15 @@ static const clivalue_t valueTable[] = { { "gps_auto_config", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->autoConfig, .config.lookup = { TABLE_OFF_ON } }, { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsConfig()->autoBaud, .config.lookup = { TABLE_OFF_ON } }, - { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], .config.minmax = { 0, 200 } }, - { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDPOS], .config.minmax = { 0, 200 } }, - { "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDPOS], .config.minmax = { 0, 200 } }, - { "gps_posr_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOSR], .config.minmax = { 0, 200 } }, - { "gps_posr_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDPOSR], .config.minmax = { 0, 200 } }, - { "gps_posr_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDPOSR], .config.minmax = { 0, 200 } }, - { "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDNAVR], .config.minmax = { 0, 200 } }, - { "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDNAVR], .config.minmax = { 0, 200 } }, - { "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDNAVR], .config.minmax = { 0, 200 } }, + { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDPOS], .config.minmax = { 0, 200 } }, + { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDPOS], .config.minmax = { 0, 200 } }, + { "gps_pos_d", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDPOS], .config.minmax = { 0, 200 } }, + { "gps_posr_p", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDPOSR], .config.minmax = { 0, 200 } }, + { "gps_posr_i", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDPOSR], .config.minmax = { 0, 200 } }, + { "gps_posr_d", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDPOSR], .config.minmax = { 0, 200 } }, + { "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDNAVR], .config.minmax = { 0, 200 } }, + { "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDNAVR], .config.minmax = { 0, 200 } }, + { "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->gps_wp_radius, .config.minmax = { 0, 2000 } }, { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsProfile()->nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->nav_speed_min, .config.minmax = { 10, 2000 } }, @@ -714,8 +714,8 @@ static const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } }, { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &mixerConfig()->yaw_motor_direction, .config.minmax = { -1, 1 } }, - { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, - { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, + { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &pidProfiles(0)->yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, + { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &pidProfiles(0)->pidSumLimit, .config.minmax = { 0.1, 1.0 } }, #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->dev.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, @@ -768,47 +768,47 @@ static const clivalue_t valueTable[] = { { "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 - { "d_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, - { "d_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 16000 } }, - { "d_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 16000 } }, - { "d_notch_cut", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, - { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, - { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, - { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, - { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, - { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, - { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 254 } }, - { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, - { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, + { "d_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &pidProfiles(0)->dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "d_lowpass", VAR_INT16 | PROFILE_VALUE, &pidProfiles(0)->dterm_lpf_hz, .config.minmax = {0, 16000 } }, + { "d_notch_hz", VAR_UINT16 | PROFILE_VALUE, &pidProfiles(0)->dterm_notch_hz, .config.minmax = { 0, 16000 } }, + { "d_notch_cut", VAR_UINT16 | PROFILE_VALUE, &pidProfiles(0)->dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, + { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &pidProfiles(0)->vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, + { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &pidProfiles(0)->pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, + { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &pidProfiles(0)->itermThrottleThreshold, .config.minmax = {20, 1000 } }, + { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &pidProfiles(0)->itermAcceleratorGain, .config.minmax = {1, 30 } }, + { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->setpointRelaxRatio, .config.minmax = {0, 100 } }, + { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->dtermSetpointWeight, .config.minmax = {0, 254 } }, + { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &pidProfiles(0)->yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, + { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &pidProfiles(0)->rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, - { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {30, 100 } }, - { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, + { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->itermWindupPointPercent, .config.minmax = {30, 100 } }, + { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &pidProfiles(0)->yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, MAX_PID_PROCESS_DENOM } }, - { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } }, - { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } }, - { "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0, 200 } }, - { "p_roll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[ROLL], .config.minmax = { 0, 200 } }, - { "i_roll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[ROLL], .config.minmax = { 0, 200 } }, - { "d_roll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[ROLL], .config.minmax = { 0, 200 } }, - { "p_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[YAW], .config.minmax = { 0, 200 } }, - { "i_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[YAW], .config.minmax = { 0, 200 } }, - { "d_yaw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[YAW], .config.minmax = { 0, 200 } }, + { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PITCH], .config.minmax = { 0, 200 } }, + { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PITCH], .config.minmax = { 0, 200 } }, + { "d_pitch", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PITCH], .config.minmax = { 0, 200 } }, + { "p_roll", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[ROLL], .config.minmax = { 0, 200 } }, + { "i_roll", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[ROLL], .config.minmax = { 0, 200 } }, + { "d_roll", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[ROLL], .config.minmax = { 0, 200 } }, + { "p_yaw", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[YAW], .config.minmax = { 0, 200 } }, + { "i_yaw", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[YAW], .config.minmax = { 0, 200 } }, + { "d_yaw", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[YAW], .config.minmax = { 0, 200 } }, - { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], .config.minmax = { 0, 200 } }, - { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], .config.minmax = { 0, 200 } }, - { "d_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDALT], .config.minmax = { 0, 200 } }, + { "p_alt", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDALT], .config.minmax = { 0, 200 } }, + { "i_alt", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDALT], .config.minmax = { 0, 200 } }, + { "d_alt", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDALT], .config.minmax = { 0, 200 } }, - { "p_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDLEVEL], .config.minmax = { 0, 200 } }, - { "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], .config.minmax = { 0, 200 } }, - { "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 200 } }, + { "p_level", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDLEVEL], .config.minmax = { 0, 200 } }, + { "i_level", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDLEVEL], .config.minmax = { 0, 200 } }, + { "d_level", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDLEVEL], .config.minmax = { 0, 200 } }, - { "p_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDVEL], .config.minmax = { 0, 200 } }, - { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, - { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, + { "p_vel", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDVEL], .config.minmax = { 0, 200 } }, + { "i_vel", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDVEL], .config.minmax = { 0, 200 } }, + { "d_vel", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDVEL], .config.minmax = { 0, 200 } }, - { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10, 200 } }, - { "level_limit", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10, 120 } }, + { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->levelSensitivity, .config.minmax = { 10, 200 } }, + { "level_limit", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->levelAngleLimit, .config.minmax = { 10, 120 } }, #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } }, @@ -1304,7 +1304,7 @@ static uint16_t getValueOffset(const clivalue_t *value) case PROFILE_VALUE: return value->offset; case PROFILE_RATE_VALUE: - return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); } return 0; } @@ -1318,7 +1318,7 @@ static void *getValuePointer(const clivalue_t *value) case PROFILE_VALUE: return rec->address + value->offset; case PROFILE_RATE_VALUE: - return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex(); } return NULL; } @@ -1370,7 +1370,7 @@ void *getValuePointer(const clivalue_t *value) } if ((value->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * systemConfig()->current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * systemConfig()->current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfileIndex()); } return ptr; @@ -3402,7 +3402,7 @@ static void cliPlaySound(char *cmdline) static void cliProfile(char *cmdline) { if (isEmpty(cmdline)) { - cliPrintf("profile %d\r\n", getCurrentProfile()); + cliPrintf("profile %d\r\n", getCurrentProfileIndex()); return; } else { const int i = atoi(cmdline); @@ -3418,7 +3418,7 @@ static void cliProfile(char *cmdline) static void cliRateProfile(char *cmdline) { if (isEmpty(cmdline)) { - cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfile()); + cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfileIndex()); return; } else { const int i = atoi(cmdline); @@ -4127,38 +4127,36 @@ static void printConfig(char *cmdline, bool doDiff) dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { - const uint8_t activeProfile = systemConfig()->current_profile_index; - for (uint32_t profileCount=0; profileCountactiveRateProfile; - for (uint32_t rateCount = 0; rateCount < MAX_RATEPROFILES; rateCount++) { - cliDumpRateProfile(rateCount, dumpMask, &defaultConfig); - } - - changeControlRateProfile(currentRateIndex); - cliPrintHashLine("restore original rateprofile selection"); - cliRateProfile(""); + const uint8_t profileIndexSave = getCurrentProfileIndex(); + for (uint32_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) { + cliDumpProfile(profileIndex, dumpMask, &defaultConfig); } - - changeProfile(activeProfile); + changeProfile(profileIndexSave); cliPrintHashLine("restore original profile selection"); cliProfile(""); + const uint8_t controlRateProfileIndexSave = getCurrentControlRateProfileIndex(); + for (uint32_t rateIndex = 0; rateIndex < MAX_RATEPROFILES; rateIndex++) { + cliDumpRateProfile(rateIndex, dumpMask, &defaultConfig); + } + changeControlRateProfile(controlRateProfileIndexSave); + cliPrintHashLine("restore original rateprofile selection"); + cliRateProfile(""); + cliPrintHashLine("save configuration"); cliPrint("save"); } else { - cliDumpProfile(systemConfig()->current_profile_index, dumpMask, &defaultConfig); - cliDumpRateProfile(systemConfig()->activeRateProfile, dumpMask, &defaultConfig); + cliDumpProfile(getCurrentProfileIndex(), dumpMask, &defaultConfig); + cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask, &defaultConfig); } } if (dumpMask & DUMP_PROFILE) { - cliDumpProfile(systemConfig()->current_profile_index, dumpMask, &defaultConfig); + cliDumpProfile(getCurrentProfileIndex(), dumpMask, &defaultConfig); } if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(systemConfig()->activeRateProfile, dumpMask, &defaultConfig); + cliDumpRateProfile(getCurrentControlRateProfileIndex(), dumpMask, &defaultConfig); } #ifdef USE_PARAMETER_GROUPS // restore configs from copies diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 41440c8895..f4a6162fce 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -107,7 +107,6 @@ master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; -static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; #ifndef USE_PARAMETER_GROUPS @@ -715,32 +714,30 @@ void resetFlashConfig(flashConfig_t *flashConfig) } #endif -uint8_t getCurrentProfile(void) +uint8_t getCurrentProfileIndex(void) { return systemConfig()->current_profile_index; -; } static void setProfile(uint8_t profileIndex) { - currentProfile = &masterConfig.profile[profileIndex]; + if (profileIndex < MAX_PROFILE_COUNT) { + systemConfigMutable()->current_profile_index = profileIndex; + currentProfile = &masterConfig.profile[profileIndex]; + } } -uint8_t getCurrentControlRateProfile(void) +uint8_t getCurrentControlRateProfileIndex(void) { - return currentControlRateProfileIndex; + return systemConfigMutable()->activeRateProfile; } static void setControlRateProfile(uint8_t controlRateProfileIndex) { - systemConfigMutable()->activeRateProfile = controlRateProfileIndex; - currentControlRateProfileIndex = controlRateProfileIndex; - currentControlRateProfile = controlRateProfilesMutable(controlRateProfileIndex); -} - -const controlRateConfig_t *getControlRateConfig(uint8_t controlRateProfileIndex) -{ - return controlRateProfiles(controlRateProfileIndex); + if (controlRateProfileIndex < MAX_CONTROL_RATE_PROFILE_COUNT) { + systemConfigMutable()->activeRateProfile = controlRateProfileIndex; + currentControlRateProfile = controlRateProfilesMutable(controlRateProfileIndex); + } } uint16_t getCurrentMinthrottle(void) @@ -1256,7 +1253,7 @@ void readEEPROM(void) failureMode(FAILURE_INVALID_EEPROM_CONTENTS); } -// pgActivateProfile(getCurrentProfile()); +// pgActivateProfile(getCurrentProfileIndex()); // setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); if (systemConfig()->current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 6303b4a7c2..2921d95005 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -101,8 +101,6 @@ void setBeeperOffMask(uint32_t mask); uint32_t getPreferredBeeperOffMask(void); void setPreferredBeeperOffMask(uint32_t mask); -void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); - void initEEPROM(void); void resetEEPROM(void); void readEEPROM(void); @@ -114,13 +112,14 @@ void validateAndFixConfig(void); void validateAndFixGyroConfig(void); void activateConfig(void); -uint8_t getCurrentProfile(void); +uint8_t getCurrentProfileIndex(void); void changeProfile(uint8_t profileIndex); struct profile_s; void resetProfile(struct profile_s *profile); -uint8_t getCurrentControlRateProfile(void); +uint8_t getCurrentControlRateProfileIndex(void); void changeControlRateProfile(uint8_t profileIndex); + bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 21cd1673a2..dbfa2407da 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -612,10 +612,10 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif 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, getCurrentProfile()); + sbufWriteU8(dst, getCurrentProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); sbufWriteU8(dst, MAX_PROFILE_COUNT); - sbufWriteU8(dst, getCurrentControlRateProfile()); + sbufWriteU8(dst, getCurrentControlRateProfileIndex()); break; case MSP_NAME: @@ -636,7 +636,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif 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, systemConfig()->current_profile_index); + sbufWriteU8(dst, getCurrentProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); sbufWriteU16(dst, 0); // gyro cycle time break; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 31c365a28a..b73ab222c9 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -362,7 +362,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) switch(adjustmentFunction) { case ADJUSTMENT_RATE_PROFILE: - if (getCurrentControlRateProfile() != position) { + if (getCurrentControlRateProfileIndex() != position) { changeControlRateProfile(position); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); applied = true; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 0a38cfe88f..f4302bfbba 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -42,11 +42,10 @@ #include "common/axis.h" #include "common/typeconversion.h" -#include "sensors/battery.h" -#include "sensors/sensors.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -55,26 +54,22 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" - -#include "io/displayport_oled.h" - -#ifdef GPS -#include "io/gps.h" #include "flight/navigation.h" -#endif - -#include "config/feature.h" -#include "config/config_profile.h" +#include "io/gps.h" #include "io/dashboard.h" +#include "io/displayport_oled.h" #include "rx/rx.h" #include "scheduler/scheduler.h" -extern profile_t *currentProfile; +#include "sensors/acceleration.h" +#include "sensors/battery.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" -controlRateConfig_t *getControlRateConfig(uint8_t profileIndex); #define MICROSECONDS_IN_A_SECOND (1000 * 1000) @@ -321,7 +316,7 @@ void showProfilePage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; - tfp_sprintf(lineBuffer, "Profile: %d", getCurrentProfile()); + tfp_sprintf(lineBuffer, "Profile: %d", getCurrentProfileIndex()); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); @@ -339,12 +334,12 @@ void showProfilePage(void) i2c_OLED_send_string(lineBuffer); } - const uint8_t currentRateProfileIndex = getCurrentControlRateProfile(); + const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex(); tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - const controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex); + const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex); tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d", controlRateConfig->rcExpo8, controlRateConfig->rcRate8 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b2a07f7c2f..033ede387b 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -393,8 +393,8 @@ static void osdDrawSingleElement(uint8_t item) case OSD_PIDRATE_PROFILE: { - const uint8_t profileIndex = systemConfig()->current_profile_index; - const uint8_t rateProfileIndex = systemConfig()->activeRateProfile; + const uint8_t profileIndex = getCurrentProfileIndex(); + const uint8_t rateProfileIndex = getCurrentControlRateProfileIndex(); sprintf(buff, "%d-%d", profileIndex + 1, rateProfileIndex + 1); break; } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index e9f8c64146..f2685796a5 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -598,7 +598,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) junk |= 1 << i; } bstWrite32(junk); - bstWrite8(systemConfig()->current_profile_index); + bstWrite8(getCurrentProfileIndex()); break; case BST_RAW_IMU: { From c92679f45439cf4d69d431554c17a8176ee458d7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 25 Feb 2017 17:35:00 +0000 Subject: [PATCH 08/27] Added PG config definitions 4 --- src/main/config/parameter_group_ids.h | 6 +++-- src/main/fc/config.c | 31 +++++++++++++++------- src/main/fc/fc_core.c | 7 +++++ src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_adjustments.c | 2 ++ src/main/fc/rc_controls.c | 11 ++++++++ src/main/flight/imu.c | 11 ++++++++ src/main/flight/mixer.c | 38 +++++++++++++++++++++++++++ src/main/flight/mixer.h | 2 ++ src/main/flight/pid.c | 13 +++++++++ 10 files changed, 110 insertions(+), 13 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 0d6d189f63..b42d360939 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -83,8 +83,10 @@ #define PG_BETAFLIGHT_START 500 #define PG_MODE_ACTIVATION_OPERATOR_CONFIG 500 #define PG_OSD_CONFIG 501 -#define PG_BEEPER_CONFIG 5002 -#define PG_BETAFLIGHT_END 1002 +#define PG_BEEPER_CONFIG 502 +#define PG_PID_CONFIG 503 +#define PG_STATUS_LED_CONFIG 504 +#define PG_BETAFLIGHT_END 504 // OSD configuration (subject to change) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 00795d43e4..1cc87d1dbc 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -104,9 +104,6 @@ #define RX_SPI_DEFAULT_PROTOCOL 0 #endif -#define BRUSHED_MOTORS_PWM_RATE 16000 -#define BRUSHLESS_MOTORS_PWM_RATE 480 - PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, @@ -125,6 +122,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; @@ -294,6 +292,7 @@ void resetServoConfig(servoConfig_t *servoConfig) } #endif +#ifndef USE_PARAMETER_GROUPS void resetMotorConfig(motorConfig_t *motorConfig) { #ifdef BRUSHED_MOTORS @@ -328,6 +327,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) } } } +#endif #ifdef SONAR void resetSonarConfig(sonarConfig_t *sonarConfig) @@ -676,6 +676,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) } #endif +#ifndef USE_PARAMETER_GROUPS void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->deadband = 0; @@ -683,6 +684,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) rcControlsConfig->alt_hold_deadband = 40; rcControlsConfig->alt_hold_fast_change = 1; } +#endif #ifndef USE_PARAMETER_GROUPS void resetMixerConfig(mixerConfig_t *mixerConfig) @@ -711,7 +713,11 @@ void resetDisplayPortProfile(displayPortProfile_t *pDisplayPortProfile) pDisplayPortProfile->rowAdjust = 0; } +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) +#else void resetStatusLedConfig(statusLedConfig_t *statusLedConfig) +#endif { for (int i = 0; i < LED_NUMBER; i++) { statusLedConfig->ledTags[i] = IO_TAG_NONE; @@ -832,8 +838,14 @@ void createDefaultConfig(master_t *config) #endif +#ifndef USE_PARAMETER_GROUPS config->imuConfig.dcm_kp = 2500; // 1.0 * 10000 config->imuConfig.dcm_ki = 0; // 0.003 * 10000 + config->imuConfig.small_angle = 25; + config->imuConfig.accDeadband.xy = 40; + config->imuConfig.accDeadband.z = 40; + config->imuConfig.acc_unarmedcal = 1; +#endif #ifndef USE_PARAMETER_GROUPS config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X @@ -888,9 +900,11 @@ void createDefaultConfig(master_t *config) resetPwmConfig(&config->pwmConfig); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef TELEMETRY resetTelemetryConfig(&config->telemetryConfig); #endif +#endif #ifdef USE_ADC resetAdcConfig(&config->adcConfig); @@ -955,15 +969,14 @@ void createDefaultConfig(master_t *config) config->armingConfig.auto_disarm_delay = 5; #endif - config->imuConfig.small_angle = 25; config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo #ifndef USE_PARAMETER_GROUPS resetMixerConfig(&config->mixerConfig); -#endif resetMotorConfig(&config->motorConfig); +#endif #ifdef USE_SERVOS resetServoConfig(&config->servoConfig); #endif @@ -993,10 +1006,6 @@ void createDefaultConfig(master_t *config) config->compassConfig.mag_declination = 0; - config->imuConfig.accDeadband.xy = 40; - config->imuConfig.accDeadband.z = 40; - config->imuConfig.acc_unarmedcal = 1; - #ifdef BARO #ifndef USE_PARAMETER_GROUPS resetBarometerConfig(&config->barometerConfig); @@ -1010,12 +1019,12 @@ void createDefaultConfig(master_t *config) parseRcChannels("AETR1234", &config->rxConfig); #endif +#ifndef USE_PARAMETER_GROUPS resetRcControlsConfig(&config->rcControlsConfig); config->throttleCorrectionConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv config->throttleCorrectionConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv -#ifndef USE_PARAMETER_GROUPS // Failsafe Variables config->failsafeConfig.failsafe_delay = 10; // 1sec config->failsafeConfig.failsafe_off_delay = 10; // 1sec @@ -1095,7 +1104,9 @@ void createDefaultConfig(master_t *config) resetFlashConfig(&config->flashConfig); #endif +#ifndef USE_PARAMETER_GROUPS resetStatusLedConfig(&config->statusLedConfig); +#endif /* merely to force a reset if the person inadvertently flashes the wrong target */ strncpy(config->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3a561ad83b..e4ce797d7c 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -102,6 +102,13 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m bool isRXDataNew; static bool armingCalibrationWasInitialised; +PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0); + +PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, + .throttle_correction_value = 0, // could 10 with althold or 40 for fpv + .throttle_correction_angle = 800 // could be 80.0 deg with atlhold or 45.0 for fpv +); + void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 3a76687c4a..357c76abf4 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -165,7 +165,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) static void taskUpdateCompass(timeUs_t currentTimeUs) { if (sensors(SENSOR_MAG)) { - compassUpdate(currentTimeUs, &compassConfig()->magZero); + compassUpdate(currentTimeUs, &compassConfigMutable()->magZero); } } #endif diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 31c365a28a..db4669d7ed 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -50,6 +50,8 @@ #include "rx/rx.h" +PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0); + static pidProfile_t *pidProfile; static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 909bc335db..58eecc4439 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -70,6 +70,15 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e +PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); + +PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, + .deadband = 0, + .yaw_deadband = 0, + .alt_hold_deadband = 40, + .alt_hold_fast_change = 1 +); + PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0); PG_RESET_TEMPLATE(armingConfig_t, armingConfig, @@ -78,6 +87,8 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig, .auto_disarm_delay = 5 ); +PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); + bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 97da5647b3..9fc7c08e29 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -76,6 +76,17 @@ static float rMat[3][3]; attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 +PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); + +PG_RESET_TEMPLATE(imuConfig_t, imuConfig, + .dcm_kp = 2500, // 1.0 * 10000 + .dcm_ki = 0, // 0.003 * 10000 + .small_angle = 25, + .accDeadband.xy = 40, + .accDeadband.z = 40, + .acc_unarmedcal = 1 +); + STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) { float q1q1 = sq(q1); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ad75905ca9..0c20a275ec 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -34,6 +34,7 @@ #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_esc_detect.h" #include "io/motors.h" @@ -69,6 +70,43 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .yaw_motor_direction = 1, ); +PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); + +void pgResetFn_motorConfig(motorConfig_t *motorConfig) +{ +#ifdef BRUSHED_MOTORS + motorConfig->minthrottle = 1000; + motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; + motorConfig->dev.useUnsyncedPwm = true; +#else +#ifdef BRUSHED_ESC_AUTODETECT + if (hardwareMotorType == MOTOR_BRUSHED) { + motorConfig->minthrottle = 1000; + motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; + motorConfig->dev.useUnsyncedPwm = true; + } else +#endif + { + motorConfig->minthrottle = 1070; + motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; + motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125; + } +#endif + motorConfig->maxthrottle = 2000; + motorConfig->mincommand = 1000; + motorConfig->digitalIdleOffsetPercent = 4.5f; + + int motorIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { + motorConfig->dev.ioTags[motorIndex] = timerHardware[i].tag; + motorIndex++; + } + } +} + #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8047d945fc..831b3dd978 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -22,6 +22,8 @@ #include "drivers/pwm_output.h" #define QUAD_MOTOR_COUNT 4 +#define BRUSHED_MOTORS_PWM_RATE 16000 +#define BRUSHLESS_MOTORS_PWM_RATE 480 /* DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 04937c42be..3a8b7c7222 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -58,6 +58,19 @@ static float previousGyroIf[3]; static float dT; +PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 0); + +#ifdef STM32F10X +#define PID_PROCESS_DENOM_DEFAULT 1 +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#define PID_PROCESS_DENOM_DEFAULT 4 +#else +#define PID_PROCESS_DENOM_DEFAULT 2 +#endif +PG_RESET_TEMPLATE(pidConfig_t, pidConfig, + .pid_process_denom = PID_PROCESS_DENOM_DEFAULT +); + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; From ee9c29c64027fe787303574f753c48a362c19103 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 26 Feb 2017 07:56:06 +1100 Subject: [PATCH 09/27] Utilised same i2c init as SPI --- src/main/drivers/bus_i2c.h | 2 +- src/main/drivers/bus_i2c_hal.c | 9 ++--- src/main/fc/fc_init.c | 47 +++++++++++++++-------- src/main/target/AIR32/target.h | 3 +- src/main/target/ALIENFLIGHTF1/target.h | 3 +- src/main/target/ALIENFLIGHTF3/target.h | 4 +- src/main/target/ALIENFLIGHTF4/target.h | 2 +- src/main/target/ANYFCF7/target.h | 5 +-- src/main/target/BLUEJAYF4/target.h | 1 + src/main/target/CHEBUZZF3/target.h | 3 +- src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI/target.h | 1 + src/main/target/COLIBRI_RACE/target.h | 6 +-- src/main/target/F4BY/target.h | 1 + src/main/target/FURYF3/target.h | 8 ++-- src/main/target/FURYF4/target.h | 3 +- src/main/target/FURYF7/target.h | 3 +- src/main/target/IRCFUSIONF3/target.h | 3 +- src/main/target/ISHAPEDF3/target.h | 3 +- src/main/target/KAKUTEF4/target.h | 8 ++-- src/main/target/KISSFC/target.h | 3 +- src/main/target/KIWIF4/target.h | 11 +++--- src/main/target/MICROSCISKY/target.h | 3 +- src/main/target/MOTOLAB/target.h | 1 + src/main/target/MULTIFLITEPICO/target.h | 3 +- src/main/target/NAZE/target.h | 3 +- src/main/target/NERO/target.h | 3 +- src/main/target/NUCLEOF7/target.h | 9 ++--- src/main/target/OMNIBUS/target.h | 3 +- src/main/target/OMNIBUSF4/target.h | 3 -- src/main/target/RCEXPLORERF3/target.h | 3 +- src/main/target/REVO/target.h | 1 + src/main/target/REVONANO/target.h | 1 + src/main/target/RG_SSD_F3/target.h | 3 +- src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SPARKY/target.h | 4 +- src/main/target/SPARKY2/target.h | 2 +- src/main/target/SPRACINGF3/target.h | 3 +- src/main/target/SPRACINGF3EVO/target.h | 3 +- src/main/target/SPRACINGF3MINI/target.h | 3 +- src/main/target/SPRACINGF3MINI/target.mk | 2 +- src/main/target/SPRACINGF3NEO/target.h | 3 +- src/main/target/STM32F3DISCOVERY/target.h | 1 + src/main/target/VRRACE/target.h | 7 ++-- src/main/target/X_RACERSPI/target.h | 3 +- src/main/target/YUPIF4/target.h | 3 +- 46 files changed, 121 insertions(+), 82 deletions(-) diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index c207034a5d..f1f687833b 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -33,7 +33,7 @@ typedef enum I2CDevice { I2CDEV_1 = 0, I2CDEV_2, I2CDEV_3, -#ifdef USE_I2C4 +#ifdef USE_I2C_DEVICE_4 I2CDEV_4, #endif I2CDEV_COUNT diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 528774e291..f82de7099a 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -62,7 +62,7 @@ static void i2cUnstick(IO_t scl, IO_t sda); #define I2C3_SDA PB4 #endif -#if defined(USE_I2C4) +#if defined(USE_I2C_DEVICE_4) #ifndef I2C4_SCL #define I2C4_SCL PD12 #endif @@ -75,12 +75,11 @@ static i2cDevice_t i2cHardwareMap[] = { { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = I2C1_OVERCLOCK, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn, .af = GPIO_AF4_I2C1 }, { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 }, { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 }, -#if defined(USE_I2C4) +#if defined(USE_I2C_DEVICE_4) { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1(I2C4), .overClock = I2C2_OVERCLOCK, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 } #endif }; - typedef struct{ I2C_HandleTypeDef Handle; }i2cHandle_t; @@ -116,7 +115,7 @@ void I2C3_EV_IRQHandler(void) HAL_I2C_EV_IRQHandler(&i2cHandle[I2CDEV_3].Handle); } -#ifdef USE_I2C4 +#ifdef USE_I2C_DEVICE_4 void I2C4_ER_IRQHandler(void) { HAL_I2C_ER_IRQHandler(&i2cHandle[I2CDEV_4].Handle); @@ -198,7 +197,7 @@ void i2cInit(I2CDevice device) case I2CDEV_3: __HAL_RCC_I2C3_CLK_ENABLE(); break; -#ifdef USE_I2C4 +#ifdef USE_I2C_DEVICE_4 case I2CDEV_4: __HAL_RCC_I2C4_CLK_ENABLE(); break; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 9846d9bbdb..a12da49038 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -323,25 +323,38 @@ void init(void) #ifdef TARGET_BUS_INIT targetBusInit(); #else - #ifdef USE_SPI - #ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); - #endif - #ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); - #endif - #ifdef USE_SPI_DEVICE_3 - spiInit(SPIDEV_3); - #endif - #ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); - #endif - #endif - #ifdef USE_I2C - i2cInit(I2C_DEVICE); - #endif +#ifdef USE_SPI +#ifdef USE_SPI_DEVICE_1 + spiInit(SPIDEV_1); #endif +#ifdef USE_SPI_DEVICE_2 + spiInit(SPIDEV_2); +#endif +#ifdef USE_SPI_DEVICE_3 + spiInit(SPIDEV_3); +#endif +#ifdef USE_SPI_DEVICE_4 + spiInit(SPIDEV_4); +#endif +#endif /* USE_SPI */ + +#ifdef USE_I2C +#ifdef USE_I2C_DEVICE_1 + i2cInit(I2CDEV_1); +#endif +#ifdef USE_I2C_DEVICE_2 + i2cInit(I2CDEV_2); +#endif +#ifdef USE_I2C_DEVICE_3 + i2cInit(I2CDEV_3); +#endif +#ifdef USE_I2C_DEVICE_4 + i2cInit(I2CDEV_4); +#endif +#endif /* USE_I2C */ + +#endif /* TARGET_BUS_INIT */ #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index e7afb03ce6..e290fabaa4 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -76,7 +76,8 @@ #define UART3_RX_PIN PB11 //(AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PA9 #define I2C2_SDA PA10 diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 56f73d71b0..570db4238e 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -53,7 +53,8 @@ #define UART3_TX_PIN PB10 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define USE_ADC #define CURRENT_METER_ADC_PIN PB1 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 4b6386f8a8..41e41bff7a 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -90,8 +90,8 @@ #define UART3_RX_PIN PB11 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) - +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PA9 #define I2C2_SDA PA10 diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index cecd4bc2c9..52c640c89a 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -140,8 +140,8 @@ #define USE_SPI_DEVICE_3 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) -//#define I2C_DEVICE_EXT (I2CDEV_2) #define I2C1_SCL PB6 #define I2C1_SDA PB7 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 6dd3ecc52d..8c4600d8cd 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -134,9 +134,8 @@ #define SDCARD_DMA_CHANNEL DMA_CHANNEL_4 #define USE_I2C -#define USE_I2C4 -#define I2C_DEVICE (I2CDEV_4) -//#define I2C_DEVICE_EXT (I2CDEV_2) +#define USE_I2C_DEVICE_4 +#define I2C_DEVICE (I2CDEV_4) #define USE_ADC #define VBAT_ADC_PIN PC0 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 482f3ee7f3..817e9b2d75 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -138,6 +138,7 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index e141d69ffa..c44f461412 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -100,7 +100,8 @@ #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index cd10384e3c..e0ad840930 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -44,6 +44,7 @@ #define SERIAL_PORT_COUNT 2 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) // #define SOFT_I2C // enable to test software i2c diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 9cf92bd1a4..7c82523e6f 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -116,6 +116,7 @@ #define SPI2_MOSI_PIN PC3 #define USE_I2C +#define USE_I2C_DEVICE_3 #define I2C_DEVICE (I2CDEV_3) #define I2C3_SCL PA8 #define I2C3_SDA PC9 diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index cb7f26331e..ce232c883b 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -99,13 +99,13 @@ #define UART3_RX_PIN PB11 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) - +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL_PIN PA9 #define I2C2_SDA_PIN PA10 #define USE_BST -#define BST_DEVICE (BSTDEV_1) +#define BST_DEVICE (BSTDEV_1) /* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */ #define BST_CRC_POLYNOM 0xD5 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 39199d241f..63457ab391 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -129,6 +129,7 @@ #define USE_I2C +#define USE_I2C_DEVICE_2 #define I2C_DEVICE (I2CDEV_2) #define USE_I2C_PULLUP #define I2C2_SCL PB10 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 4c95f8959b..24cc5df743 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -135,10 +135,10 @@ #define SOFTSERIAL1_TX_PIN PB1 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) - -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index ae67882d9d..bd94c486c9 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -155,7 +155,8 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP #define I2C1_SCL PB6 #define I2C1_SDA PB7 diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index ff108c7c8d..cb8862cb71 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -124,7 +124,8 @@ #define SERIAL_PORT_COUNT 6 //VCP, USART1, USART3, USART6, SOFTSERIAL x 2 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP #define I2C1_SCL PB6 #define I2C1_SDA PB7 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 5ca1bbf336..2291bb78a7 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -63,7 +63,8 @@ #define UART3_RX_PIN PB11 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h index fcd61d8334..105a194843 100644 --- a/src/main/target/ISHAPEDF3/target.h +++ b/src/main/target/ISHAPEDF3/target.h @@ -80,7 +80,8 @@ #define SOFTSERIAL2_TX_PIN PB1 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8c4deb0e6a..5f502aa880 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -114,15 +114,15 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) -//#define I2C_DEVICE_EXT (I2CDEV_2) #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC -#define VBAT_ADC_PIN PC3 -#define VBAT_ADC_CHANNEL ADC_Channel_13 +#define VBAT_ADC_PIN PC3 +#define VBAT_ADC_CHANNEL ADC_Channel_13 -#define CURRENT_METER_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC2 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_12 #define RSSI_ADC_PIN PC1 diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 01851f3d55..272c36db01 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -85,7 +85,8 @@ #endif #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_ADC #define VBAT_SCALE_DEFAULT 160 diff --git a/src/main/target/KIWIF4/target.h b/src/main/target/KIWIF4/target.h index 2f5b4af279..8540aaef4e 100644 --- a/src/main/target/KIWIF4/target.h +++ b/src/main/target/KIWIF4/target.h @@ -105,13 +105,14 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 - - -/* #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +/* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP #define I2C1_SCL PB6 -#define I2C1_SDA PB7 */ +#define I2C1_SDA PB7 +*/ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 11425cfa80..9fc3dcad58 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -67,7 +67,8 @@ #define SERIAL_PORT_COUNT 2 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index e78417d1da..df7deaf56b 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -76,6 +76,7 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C +#define USE_I2C_DEVICE_2 #define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PA9 #define I2C2_SDA PA10 diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index a03a113a1a..2b3d65b447 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/src/main/target/MULTIFLITEPICO/target.h @@ -90,7 +90,8 @@ */ #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 6b99305e96..973b01a3d7 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -129,7 +129,8 @@ #define UART3_TX_PIN PB10 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) // #define SOFT_I2C // enable to test software i2c // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index 85aa854b13..406a7030c7 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -70,7 +70,8 @@ #define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_VCP //#define VBUS_SENSING_PIN PA8 diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index 3d4a472b84..6e8804aa6f 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -133,11 +133,10 @@ #define SDCARD_DMA_CHANNEL DMA_CHANNEL_4 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - -//#define I2C_DEVICE_EXT (I2CDEV_2) +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 #define USE_ADC #define VBAT_ADC_PIN PA3 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index d88c343d55..d43a2f705a 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -82,7 +82,8 @@ #undef USE_I2C //#define USE_I2C -//#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +//#define USE_I2C_DEVICE_1 +//#define I2C_DEVICE (I2CDEV_1) #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 1e55afd7f3..68e37d297f 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -155,9 +155,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -//#define USE_I2C -//#define I2C_DEVICE (I2CDEV_1) - #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 #define VBAT_ADC_PIN PC2 diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index fda731bb3c..3d9becdfaa 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -82,7 +82,8 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PA9 #define I2C2_SDA PA10 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 9c7c0c01a2..4cf0ab2740 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -176,6 +176,7 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) #define USE_ADC diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 998f7ac0c2..c0db4ddf9d 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -76,6 +76,7 @@ #define USE_SPI_DEVICE_2 #define USE_I2C +#define USE_I2C_DEVICE_3 #define I2C_DEVICE (I2CDEV_3) #undef LED_STRIP diff --git a/src/main/target/RG_SSD_F3/target.h b/src/main/target/RG_SSD_F3/target.h index afe500fec6..4d600d3c41 100644 --- a/src/main/target/RG_SSD_F3/target.h +++ b/src/main/target/RG_SSD_F3/target.h @@ -103,7 +103,8 @@ #define UART5_RX_PIN PD2 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // SDA (PA14/AF4), SCL (PA15/AF4) +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL_GPIO GPIOA #define I2C2_SCL_GPIO_AF GPIO_AF_4 diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index dd9069e715..261293a32e 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -65,7 +65,8 @@ #define SOFTSERIAL1_TX_PIN PA3 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_1 // PA4, 5, 6, 7 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index a1c7f97f60..06180886cf 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -77,8 +77,8 @@ // Note: PA5 and PA0 are N/C on the sparky - potentially use for ADC or LED STRIP? #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) - +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PA9 #define I2C2_SDA PA10 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index a516a8f469..0b3c64e526 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -114,8 +114,8 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) -//#define I2C_DEVICE_EXT (I2CDEV_2) #define USE_ADC #define VBAT_ADC_PIN PC3 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index f369576eb2..f774fb6639 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -154,7 +154,8 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 3fbf7f0e33..374eb1a66d 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -92,7 +92,8 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index c859204012..1f1712e0b2 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -129,7 +129,8 @@ #define MPU6500_SPI_INSTANCE SPI1 #else #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 diff --git a/src/main/target/SPRACINGF3MINI/target.mk b/src/main/target/SPRACINGF3MINI/target.mk index a46cf1be8f..386adddd92 100644 --- a/src/main/target/SPRACINGF3MINI/target.mk +++ b/src/main/target/SPRACINGF3MINI/target.mk @@ -7,7 +7,7 @@ TARGET_SRC = \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ drivers/compass_hmc5883l.c \ - drivers/compass_ak8963.c \ + drivers/compass_ak8963.c \ drivers/flash_m25p16.c \ drivers/transponder_ir.c \ drivers/transponder_ir_stm32f30x.c \ diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index eba7318c4c..4cf44851d0 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -78,7 +78,8 @@ #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_1 // MPU diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 7c4cdb2b21..ddc077ca0f 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -163,6 +163,7 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C +#define USE_I2C_DEVICE_1 #define I2C_DEVICE (I2CDEV_1) #define USE_ADC diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index 89aa9f696f..5ef82fcc5c 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -145,10 +145,11 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB8-SCL, PB8-SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 */ #define USE_ADC diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 585c8bfcdf..2c4b77a443 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -75,7 +75,8 @@ #define SONAR_SOFTSERIAL1_EXCLUSIVE #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 461af8aefd..c34fd98127 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -132,7 +132,8 @@ #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // SCL PB8 - SDA PB9 +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE (I2CDEV_1) // ADC inputs #define BOARD_HAS_VOLTAGE_DIVIDER From 1436be2acf11d1c4c03abccdc07ee3ef18fae254 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 26 Feb 2017 13:10:12 +1300 Subject: [PATCH 10/27] Fixed build error if TRANSPONDER is undefined. --- src/main/io/transponder_ir.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index a5c81ab020..b52bf0a1f2 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -23,6 +23,7 @@ #include +#ifdef TRANSPONDER #include "build/build_config.h" #include "config/parameter_group.h" @@ -116,3 +117,4 @@ void transponderTransmitOnce(void) { } transponderIrTransmit(); } +#endif From 46d77e3316f2c28d1f59683527c2342686d87673 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 26 Feb 2017 17:57:48 +1300 Subject: [PATCH 11/27] Added transponder driver for F4. --- src/main/drivers/transponder_ir_stm32f4xx.c | 146 ++++++++++++++++++++ 1 file changed, 146 insertions(+) create mode 100644 src/main/drivers/transponder_ir_stm32f4xx.c diff --git a/src/main/drivers/transponder_ir_stm32f4xx.c b/src/main/drivers/transponder_ir_stm32f4xx.c new file mode 100644 index 0000000000..fc89b5b540 --- /dev/null +++ b/src/main/drivers/transponder_ir_stm32f4xx.c @@ -0,0 +1,146 @@ +/* + * 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 + +#include "io.h" +#include "nvic.h" + +#include "dma.h" +#include "rcc.h" +#include "timer.h" +#include "timer_stm32f4xx.h" + +#include "transponder_ir.h" + +static IO_t transponderIO = IO_NONE; +static DMA_Stream_TypeDef *stream = NULL; +static TIM_TypeDef *timer = NULL; + +static void TRANSPONDER_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) +{ + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + transponderIrDataTransferInProgress = 0; + DMA_Cmd(descriptor->stream, DISABLE); + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + } +} + + +void transponderIrHardwareInit(ioTag_t ioTag) +{ + if (!ioTag) { + return; + } + + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_OCInitTypeDef TIM_OCInitStructure; + DMA_InitTypeDef DMA_InitStructure; + + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; + + if (timerHardware->dmaStream == NULL) { + return; + } + + transponderIO = IOGetByTag(ioTag); + IOInit(transponderIO, OWNER_TRANSPONDER, 0); + IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); + + dmaInit(timerHardware->dmaIrqHandler, OWNER_TRANSPONDER, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, 0); + + RCC_ClockCmd(timerRCC(timer), ENABLE); + + /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = 156; + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); + + /* PWM1 Mode configuration: Channel1 */ + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + } + + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_Pulse = 0; + + timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); + timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); + + TIM_CtrlPWMOutputs(timer, ENABLE); + + /* configure DMA */ + stream = timerHardware->dmaStream; + DMA_Cmd(stream, DISABLE); + DMA_DeInit(stream); + + DMA_StructInit(&DMA_InitStructure); + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)transponderIrDMABuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_BufferSize = TRANSPONDER_DMA_BUFFER_SIZE; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + + DMA_Init(stream, &DMA_InitStructure); + + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); + + DMA_ITConfig(stream, DMA_IT_TC, ENABLE); +} + +void transponderIrDMAEnable(void) +{ + DMA_SetCurrDataCounter(stream, TRANSPONDER_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); + DMA_Cmd(stream, ENABLE); +} + +void transponderIrDisable(void) +{ + DMA_Cmd(stream, DISABLE); + TIM_Cmd(timer, DISABLE); + + IOInit(transponderIO, OWNER_TRANSPONDER, 0); + IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); + +#ifdef TRANSPONDER_INVERTED + IOHi(transponderIO); +#else + IOLo(transponderIO); +#endif +} + From 76a6a5d306ae265b1487dae3105824f5aee203b6 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 26 Feb 2017 18:54:25 +1300 Subject: [PATCH 12/27] Re-added check to reset EEPROM config if board identifier has changed. --- src/main/config/config_eeprom.c | 9 ++++++++- src/main/config/config_master.h | 3 --- src/main/fc/config.c | 3 --- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 295cd77b97..c95e552488 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -62,6 +62,7 @@ typedef enum { // Header for the saved copy. typedef struct { uint8_t format; + char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER)]; } PG_PACKED configHeader_t; // Header for each stored PG. @@ -96,7 +97,7 @@ void initEEPROM(void) BUILD_BUG_ON(offsetof(packingTest_t, word) != 1); BUILD_BUG_ON(sizeof(packingTest_t) != 5); - BUILD_BUG_ON(sizeof(configHeader_t) != 1); + BUILD_BUG_ON(sizeof(configHeader_t) != 1 + sizeof(TARGET_BOARD_IDENTIFIER)); BUILD_BUG_ON(sizeof(configFooter_t) != 2); BUILD_BUG_ON(sizeof(configRecord_t) != 6); } @@ -122,6 +123,11 @@ bool isEEPROMContentValid(void) if (header->format != EEPROM_CONF_VERSION) { return false; } + + if (strncasecmp(header->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) { + return false; + } + chk = updateChecksum(chk, header, sizeof(*header)); p += sizeof(*header); // include the transitional masterConfig record @@ -227,6 +233,7 @@ static bool writeSettingsToEEPROM(void) configHeader_t header = { .format = EEPROM_CONF_VERSION, + .boardIdentifier = TARGET_BOARD_IDENTIFIER, }; config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4f24b177bb..c458172a1d 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -320,9 +320,6 @@ typedef struct master_s { beeperConfig_t beeperConfig; - char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER)]; - - uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum /* do not add properties after the MAGIC_EF and CHK diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 00795d43e4..c45a8f6b11 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1097,9 +1097,6 @@ void createDefaultConfig(master_t *config) resetStatusLedConfig(&config->statusLedConfig); - /* merely to force a reset if the person inadvertently flashes the wrong target */ - strncpy(config->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER)); - #if defined(TARGET_CONFIG) targetConfiguration(config); #endif From 5fb4062a00db9e64bdea9ece1ad3b5837c1c2bf3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Feb 2017 06:37:45 +0000 Subject: [PATCH 13/27] Fixed initialisation of pid and rate profiles --- src/main/fc/config.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index f4a6162fce..6251243dcf 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -936,8 +936,13 @@ void createDefaultConfig(master_t *config) resetSerialConfig(&config->serialConfig); - resetProfile(&config->profile[0]); - resetControlRateProfile(&config->controlRateProfile[0]); + + for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { + resetProfile(&config->profile[ii]); + } + for (int ii = 0; ii < MAX_CONTROL_RATE_PROFILE_COUNT; ++ii) { + resetControlRateProfile(&config->controlRateProfile[ii]); + } config->compassConfig.mag_declination = 0; @@ -1049,11 +1054,6 @@ void createDefaultConfig(master_t *config) #if defined(TARGET_CONFIG) targetConfiguration(config); #endif - - // copy first profile into remaining profile - for (int i = 1; i < MAX_PROFILE_COUNT; i++) { - memcpy(&config->profile[i], &config->profile[0], sizeof(profile_t)); - } } void resetConfigs(void) From 88c1f1e6b3466cbafc97bda45724aa434b0ed8e5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 29 Jan 2017 20:06:55 +0000 Subject: [PATCH 14/27] Initial AlienFlightNG F7 support --- src/main/target/ALIENFLIGHTF1/AlienFlight.md | 42 ++-- src/main/target/ALIENFLIGHTF3/AlienFlight.md | 42 ++-- src/main/target/ALIENFLIGHTF4/AlienFlight.md | 42 ++-- .../target/ALIENFLIGHTNGF7/AlienFlight.md | 74 +++++++ src/main/target/ALIENFLIGHTNGF7/config.c | 100 +++++++++ .../ALIENFLIGHTNGF7/hardware_revision.c | 52 +++++ .../ALIENFLIGHTNGF7/hardware_revision.h | 28 +++ src/main/target/ALIENFLIGHTNGF7/target.c | 46 +++++ src/main/target/ALIENFLIGHTNGF7/target.h | 192 ++++++++++++++++++ src/main/target/ALIENFLIGHTNGF7/target.mk | 15 ++ 10 files changed, 591 insertions(+), 42 deletions(-) create mode 100644 src/main/target/ALIENFLIGHTNGF7/AlienFlight.md create mode 100644 src/main/target/ALIENFLIGHTNGF7/config.c create mode 100644 src/main/target/ALIENFLIGHTNGF7/hardware_revision.c create mode 100644 src/main/target/ALIENFLIGHTNGF7/hardware_revision.h create mode 100644 src/main/target/ALIENFLIGHTNGF7/target.c create mode 100644 src/main/target/ALIENFLIGHTNGF7/target.h create mode 100644 src/main/target/ALIENFLIGHTNGF7/target.mk diff --git a/src/main/target/ALIENFLIGHTF1/AlienFlight.md b/src/main/target/ALIENFLIGHTF1/AlienFlight.md index 4c33df5680..d4b20af427 100644 --- a/src/main/target/ALIENFLIGHTF1/AlienFlight.md +++ b/src/main/target/ALIENFLIGHTF1/AlienFlight.md @@ -1,15 +1,18 @@ -# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 and ALIENFLIGHTNGF7 target) -AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: +AlienWii is now AlienFlight. This designs are released for public and some for noncommercial use at: http://www.alienflight.com -http://www.alienflightng.com -AlienFlight Classic and F3 Eagle files are available at: +AlienFlight F3 Eagle files are available at: https://github.com/MJ666/Flight-Controllers -All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build this designs. +AlienFlightNG (Next Generation) designs are released for noncommercial use can be found here: + +http://www.alienflightng.com + +This targets supports various variants of brushed and brushless flight controllers. All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build them. Some variants of the AlienFlight controllers will be available for purchase from: @@ -21,8 +24,9 @@ Here are the general hardware specifications for this boards: - STM32F103CBT6 MCU (ALIENFLIGHTF1) - STM32F303CCT6 MCU (ALIENFLIGHTF3) - STM32F405RGT6 MCU (ALIENFLIGHTF4) +- STM32F711RET6 MCU (ALIENFLIGHTNGF7) - MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit -- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- The MPU sensor interrupt is connected to the MCU for all published designs and enabled in the firmware - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) - extra-wide traces on the PCB, for maximum power throughput (brushed variants) - some new F4 boards using a 4-layer PCB for better power distribution @@ -33,28 +37,38 @@ Here are the general hardware specifications for this boards: - hardware bind plug for easy binding - motor connections are at the corners for a clean look with reduced wiring - small footprint -- direct operation from a single cell Lipoly battery for brushed versions +- direct operation from a single cell LIPO battery for brushed versions - 3.3V LDO power regulator (older prototypes) - 3.3V buck-boost power converter (all new versions) - 5V buck-boost power converter for FPV (some versions) -- brushless versions are designed for 3S operation and also have an 5V power output -- battery monitoring with an LED or buzzer output (for some ALIENFLIGHTF3 and F4 variants only, F4 V2 has also current monitoring) -- SDCard Reader for black box monitoring (F4 V2 versions) -- Integrated OpenSky (FRSky compatible) receiver with FRSky hub telemetry (F4 V2 versions) +- brushless versions are designed for 4S operation and also have an 5V power output +- battery monitoring with an LED or buzzer output (for some variants only) +- current monitoring (F4/F7 V1.1 versions) +- SDCard Reader for black box monitoring (F4/F7 V1.1 versions) +- (**) integrated OpenSky (FrSky compatible) receiver with FrSky hub telemetry (F4/F7 V2 versions) +- hardware detection of brushed and brushless versions with individual defaults (*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. +(**) This receiver is based on the uSky and OpenSky projects. http://www.fishpepper.de + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) set spektrum_sat_bind = 5 For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. -Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. +Deltang receivers in serial mode will work like any other Spektrum satellite compatible receiver (10bit, 22ms) only the bind process will be different. -The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. +The pin layout for the AlienFlight F1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The pin layout for the AlienFlight F3 is similar to Sparky. The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight F3 flight controllers running the same firmware which takes care on the differences with a hardware detection. The AlienFlight F4 and F7 have their individual pin layouts and are independent designs. -The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. +(**) OpenSky receiver with telemetry is enabled by default if present on the board. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 or ALIENFLIGHTNGF7. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. ## Flashing the firmware The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. + +The firmware for the OpenSky receiver can be updated via serial pass-through and the embedded boot loader. The initial flashing need to be done with the ISP programming pins. The target for the embedded AlienFlight OpenSky receiver is "AFF4RX". Please refer to the OpenSky project for more details. + +https://github.com/fishpepper/OpenSky/blob/master/README.md diff --git a/src/main/target/ALIENFLIGHTF3/AlienFlight.md b/src/main/target/ALIENFLIGHTF3/AlienFlight.md index 4c33df5680..d4b20af427 100644 --- a/src/main/target/ALIENFLIGHTF3/AlienFlight.md +++ b/src/main/target/ALIENFLIGHTF3/AlienFlight.md @@ -1,15 +1,18 @@ -# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 and ALIENFLIGHTNGF7 target) -AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: +AlienWii is now AlienFlight. This designs are released for public and some for noncommercial use at: http://www.alienflight.com -http://www.alienflightng.com -AlienFlight Classic and F3 Eagle files are available at: +AlienFlight F3 Eagle files are available at: https://github.com/MJ666/Flight-Controllers -All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build this designs. +AlienFlightNG (Next Generation) designs are released for noncommercial use can be found here: + +http://www.alienflightng.com + +This targets supports various variants of brushed and brushless flight controllers. All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build them. Some variants of the AlienFlight controllers will be available for purchase from: @@ -21,8 +24,9 @@ Here are the general hardware specifications for this boards: - STM32F103CBT6 MCU (ALIENFLIGHTF1) - STM32F303CCT6 MCU (ALIENFLIGHTF3) - STM32F405RGT6 MCU (ALIENFLIGHTF4) +- STM32F711RET6 MCU (ALIENFLIGHTNGF7) - MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit -- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- The MPU sensor interrupt is connected to the MCU for all published designs and enabled in the firmware - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) - extra-wide traces on the PCB, for maximum power throughput (brushed variants) - some new F4 boards using a 4-layer PCB for better power distribution @@ -33,28 +37,38 @@ Here are the general hardware specifications for this boards: - hardware bind plug for easy binding - motor connections are at the corners for a clean look with reduced wiring - small footprint -- direct operation from a single cell Lipoly battery for brushed versions +- direct operation from a single cell LIPO battery for brushed versions - 3.3V LDO power regulator (older prototypes) - 3.3V buck-boost power converter (all new versions) - 5V buck-boost power converter for FPV (some versions) -- brushless versions are designed for 3S operation and also have an 5V power output -- battery monitoring with an LED or buzzer output (for some ALIENFLIGHTF3 and F4 variants only, F4 V2 has also current monitoring) -- SDCard Reader for black box monitoring (F4 V2 versions) -- Integrated OpenSky (FRSky compatible) receiver with FRSky hub telemetry (F4 V2 versions) +- brushless versions are designed for 4S operation and also have an 5V power output +- battery monitoring with an LED or buzzer output (for some variants only) +- current monitoring (F4/F7 V1.1 versions) +- SDCard Reader for black box monitoring (F4/F7 V1.1 versions) +- (**) integrated OpenSky (FrSky compatible) receiver with FrSky hub telemetry (F4/F7 V2 versions) +- hardware detection of brushed and brushless versions with individual defaults (*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. +(**) This receiver is based on the uSky and OpenSky projects. http://www.fishpepper.de + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) set spektrum_sat_bind = 5 For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. -Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. +Deltang receivers in serial mode will work like any other Spektrum satellite compatible receiver (10bit, 22ms) only the bind process will be different. -The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. +The pin layout for the AlienFlight F1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The pin layout for the AlienFlight F3 is similar to Sparky. The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight F3 flight controllers running the same firmware which takes care on the differences with a hardware detection. The AlienFlight F4 and F7 have their individual pin layouts and are independent designs. -The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. +(**) OpenSky receiver with telemetry is enabled by default if present on the board. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 or ALIENFLIGHTNGF7. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. ## Flashing the firmware The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. + +The firmware for the OpenSky receiver can be updated via serial pass-through and the embedded boot loader. The initial flashing need to be done with the ISP programming pins. The target for the embedded AlienFlight OpenSky receiver is "AFF4RX". Please refer to the OpenSky project for more details. + +https://github.com/fishpepper/OpenSky/blob/master/README.md diff --git a/src/main/target/ALIENFLIGHTF4/AlienFlight.md b/src/main/target/ALIENFLIGHTF4/AlienFlight.md index 4c33df5680..d4b20af427 100644 --- a/src/main/target/ALIENFLIGHTF4/AlienFlight.md +++ b/src/main/target/ALIENFLIGHTF4/AlienFlight.md @@ -1,15 +1,18 @@ -# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 and ALIENFLIGHTNGF7 target) -AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: +AlienWii is now AlienFlight. This designs are released for public and some for noncommercial use at: http://www.alienflight.com -http://www.alienflightng.com -AlienFlight Classic and F3 Eagle files are available at: +AlienFlight F3 Eagle files are available at: https://github.com/MJ666/Flight-Controllers -All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build this designs. +AlienFlightNG (Next Generation) designs are released for noncommercial use can be found here: + +http://www.alienflightng.com + +This targets supports various variants of brushed and brushless flight controllers. All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build them. Some variants of the AlienFlight controllers will be available for purchase from: @@ -21,8 +24,9 @@ Here are the general hardware specifications for this boards: - STM32F103CBT6 MCU (ALIENFLIGHTF1) - STM32F303CCT6 MCU (ALIENFLIGHTF3) - STM32F405RGT6 MCU (ALIENFLIGHTF4) +- STM32F711RET6 MCU (ALIENFLIGHTNGF7) - MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit -- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- The MPU sensor interrupt is connected to the MCU for all published designs and enabled in the firmware - 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) - extra-wide traces on the PCB, for maximum power throughput (brushed variants) - some new F4 boards using a 4-layer PCB for better power distribution @@ -33,28 +37,38 @@ Here are the general hardware specifications for this boards: - hardware bind plug for easy binding - motor connections are at the corners for a clean look with reduced wiring - small footprint -- direct operation from a single cell Lipoly battery for brushed versions +- direct operation from a single cell LIPO battery for brushed versions - 3.3V LDO power regulator (older prototypes) - 3.3V buck-boost power converter (all new versions) - 5V buck-boost power converter for FPV (some versions) -- brushless versions are designed for 3S operation and also have an 5V power output -- battery monitoring with an LED or buzzer output (for some ALIENFLIGHTF3 and F4 variants only, F4 V2 has also current monitoring) -- SDCard Reader for black box monitoring (F4 V2 versions) -- Integrated OpenSky (FRSky compatible) receiver with FRSky hub telemetry (F4 V2 versions) +- brushless versions are designed for 4S operation and also have an 5V power output +- battery monitoring with an LED or buzzer output (for some variants only) +- current monitoring (F4/F7 V1.1 versions) +- SDCard Reader for black box monitoring (F4/F7 V1.1 versions) +- (**) integrated OpenSky (FrSky compatible) receiver with FrSky hub telemetry (F4/F7 V2 versions) +- hardware detection of brushed and brushless versions with individual defaults (*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. +(**) This receiver is based on the uSky and OpenSky projects. http://www.fishpepper.de + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) set spektrum_sat_bind = 5 For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. -Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. +Deltang receivers in serial mode will work like any other Spektrum satellite compatible receiver (10bit, 22ms) only the bind process will be different. -The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. +The pin layout for the AlienFlight F1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The pin layout for the AlienFlight F3 is similar to Sparky. The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight F3 flight controllers running the same firmware which takes care on the differences with a hardware detection. The AlienFlight F4 and F7 have their individual pin layouts and are independent designs. -The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. +(**) OpenSky receiver with telemetry is enabled by default if present on the board. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 or ALIENFLIGHTNGF7. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. ## Flashing the firmware The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. + +The firmware for the OpenSky receiver can be updated via serial pass-through and the embedded boot loader. The initial flashing need to be done with the ISP programming pins. The target for the embedded AlienFlight OpenSky receiver is "AFF4RX". Please refer to the OpenSky project for more details. + +https://github.com/fishpepper/OpenSky/blob/master/README.md diff --git a/src/main/target/ALIENFLIGHTNGF7/AlienFlight.md b/src/main/target/ALIENFLIGHTNGF7/AlienFlight.md new file mode 100644 index 0000000000..d4b20af427 --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/AlienFlight.md @@ -0,0 +1,74 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 and ALIENFLIGHTNGF7 target) + +AlienWii is now AlienFlight. This designs are released for public and some for noncommercial use at: + +http://www.alienflight.com + +AlienFlight F3 Eagle files are available at: + +https://github.com/MJ666/Flight-Controllers + +AlienFlightNG (Next Generation) designs are released for noncommercial use can be found here: + +http://www.alienflightng.com + +This targets supports various variants of brushed and brushless flight controllers. All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users and in some cases RC vendors to build them. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu +https://micro-motor-warehouse.com + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- STM32F711RET6 MCU (ALIENFLIGHTNGF7) +- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all published designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell LIPO battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 4S operation and also have an 5V power output +- battery monitoring with an LED or buzzer output (for some variants only) +- current monitoring (F4/F7 V1.1 versions) +- SDCard Reader for black box monitoring (F4/F7 V1.1 versions) +- (**) integrated OpenSky (FrSky compatible) receiver with FrSky hub telemetry (F4/F7 V2 versions) +- hardware detection of brushed and brushless versions with individual defaults + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + +(**) This receiver is based on the uSky and OpenSky projects. http://www.fishpepper.de + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite compatible receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the AlienFlight F1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The pin layout for the AlienFlight F3 is similar to Sparky. The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight F3 flight controllers running the same firmware which takes care on the differences with a hardware detection. The AlienFlight F4 and F7 have their individual pin layouts and are independent designs. + +(**) OpenSky receiver with telemetry is enabled by default if present on the board. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3, ALIENFLIGHTF4 or ALIENFLIGHTNGF7. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. + +The firmware for the OpenSky receiver can be updated via serial pass-through and the embedded boot loader. The initial flashing need to be done with the ISP programming pins. The target for the embedded AlienFlight OpenSky receiver is "AFF4RX". Please refer to the OpenSky project for more details. + +https://github.com/fishpepper/OpenSky/blob/master/README.md diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c new file mode 100644 index 0000000000..dcbda39554 --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -0,0 +1,100 @@ +/* + * 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 TARGET_CONFIG +#include "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/compass.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "telemetry/telemetry.h" + +#include "sensors/sensors.h" +#include "sensors/compass.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#include "hardware_revision.h" + +#define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V +#define CURRENTSCALE -667 // ACS712/714-30A - 66.666 mV/A inverted mode + +#define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz + +// alternative defaults settings for AlienFlight targets +void targetConfiguration(master_t *config) +{ + config->batteryConfig.currentMeterOffset = CURRENTOFFSET; + config->batteryConfig.currentMeterScale = CURRENTSCALE; + config->compassConfig.mag_hardware = MAG_NONE; // disabled by default + + if (hardwareMotorType == MOTOR_BRUSHED) { + config->motorConfig.dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; + config->pidConfig.pid_process_denom = 1; + } + + if (hardwareRevision == AFF7_REV_1) { + config->rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048; + config->rxConfig.spektrum_sat_bind = 5; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + } else { + config->rxConfig.serialrx_provider = SERIALRX_SBUS; + config->rxConfig.sbus_inversion = 0; + config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY; + config->telemetryConfig.telemetry_inversion = 0; + intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY, &config->featureConfig.enabledFeatures); + } + + config->profile[0].pidProfile.P8[ROLL] = 53; + config->profile[0].pidProfile.I8[ROLL] = 45; + config->profile[0].pidProfile.D8[ROLL] = 52; + config->profile[0].pidProfile.P8[PITCH] = 53; + config->profile[0].pidProfile.I8[PITCH] = 45; + config->profile[0].pidProfile.D8[PITCH] = 52; + config->profile[0].pidProfile.P8[YAW] = 64; + config->profile[0].pidProfile.D8[YAW] = 18; + + config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L +} +#endif diff --git a/src/main/target/ALIENFLIGHTNGF7/hardware_revision.c b/src/main/target/ALIENFLIGHTNGF7/hardware_revision.c new file mode 100644 index 0000000000..a682d67ae2 --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/hardware_revision.c @@ -0,0 +1,52 @@ +/* + * 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 + +#include "platform.h" + +#include "build/build_config.h" + +#include "drivers/system.h" +#include "drivers/io.h" +#include "hardware_revision.h" + +uint8_t hardwareRevision = AFF7_UNKNOWN; + +static IO_t HWDetectPin = IO_NONE; + +void detectHardwareRevision(void) +{ + HWDetectPin = IOGetByTag(IO_TAG(HW_PIN)); + IOInit(HWDetectPin, OWNER_SYSTEM, 0); + IOConfigGPIO(HWDetectPin, IOCFG_IPU); + + delayMicroseconds(10); // allow configuration to settle + + // Check hardware revision + if (IORead(HWDetectPin)) { + hardwareRevision = AFF7_REV_1; + } else { + hardwareRevision = AFF7_REV_2; + } +} + +void updateHardwareRevision(void) +{ +} diff --git a/src/main/target/ALIENFLIGHTNGF7/hardware_revision.h b/src/main/target/ALIENFLIGHTNGF7/hardware_revision.h new file mode 100644 index 0000000000..5a478a7eaf --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/hardware_revision.h @@ -0,0 +1,28 @@ +/* + * 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 + +typedef enum awf7HardwareRevision_t { + AFF7_UNKNOWN = 0, + AFF7_REV_1, // ICM-20602 (SPI), Current Sensor (ACS712), SDCard Reader + AFF7_REV_2 // ICM-20602 (SPI), OpenSky RX (CC2510), Current Sensor (ACS712), SDCard Reader +} awf7HardwareRevision_e; + +extern uint8_t hardwareRevision; + +void updateHardwareRevision(void); +void detectHardwareRevision(void); diff --git a/src/main/target/ALIENFLIGHTNGF7/target.c b/src/main/target/ALIENFLIGHTNGF7/target.c new file mode 100644 index 0000000000..93a44e7444 --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/target.c @@ -0,0 +1,46 @@ +/* + * 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" + +// DSHOT will work for motor 1-8. +// If SDCard or USART4 DMA is used motor 6 will not work. +// If the ADC is used motor 7 will not work. +// If UART1 DMA is used motor 8 will not work. + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, TIMER_INPUT_ENABLED, 1), // PPM - DMA2_ST6, *DMA2_ST1, DMA2_ST3 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM1 - DMA2_ST2, DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM2 - DMA2_ST3, DMA2_ST2 + DEF_TIM(TIM1, CH2N, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED , 0), // PWM3 - DMA2_ST6, DMA2_ST2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM4 - DMA1_ST7 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM5 - DMA1_ST2 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - (DMA1_ST4) - DMA SDCard, DMA Serial_TX4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM7 - (DMA2_ST4) DMA2_ST2 - DMA ADC + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM8 - (DMA2_ST7) - DMA Serial_TX1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM9 - (DMA1_ST2) - Collision + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED , 0), // PWM10 - (DMA2_ST6), (DMA2_ST6) - Collision + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM11 - (DMA1_ST7) - Collision + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM12 - DMA_NONE +}; diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h new file mode 100644 index 0000000000..2d7bce4919 --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -0,0 +1,192 @@ +/* + * 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 TARGET_BOARD_IDENTIFIER "AFF7" +#define TARGET_CONFIG + +#define USE_HARDWARE_REVISION_DETECTION +#define HW_PIN PC13 +#define BRUSHED_ESC_AUTODETECT +#define USE_DSHOT + +#define USBD_PRODUCT_STRING "AlienFlightNG F7" + +#define LED0 PC12 +#define LED1 PD2 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// MPU interrupt +#define USE_EXTI +#define MPU_INT_EXTI PC14 +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW270_DEG + +#define MAG +#define USE_MAG_HMC5883 +#define USE_MAG_SPI_HMC5883 +#define USE_MAG_AK8963 + +#define HMC5883_CS_PIN PC15 +#define HMC5883_SPI_INSTANCE SPI3 + +#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_AK8963_ALIGN CW270_DEG + +#define BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 + +#define BMP280_CS_PIN SPI3_NSS_PIN +#define BMP280_SPI_INSTANCE SPI3 + +#define USE_SDCARD + +//#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PB11 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB +#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn + +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PB10 + +// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz + +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +//#define M25P16_CS_PIN SPI2_NSS_PIN +//#define M25P16_SPI_INSTANCE SPI2 + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 + +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +//#define USE_UART3 +//#define UART3_RX_PIN PB11 +//#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PC10 +#define UART4_TX_PIN PC11 + +//#define USE_UART5 +//#define UART5_RX_PIN PD2 +//#define UART5_TX_PIN PC12 + +#define SERIAL_PORT_COUNT 4 + +//#define USE_ESCSERIAL +//#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) +//#define I2C_DEVICE_EXT (I2CDEV_2) +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC4 + +// LED strip configuration. +#define LED_STRIP + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PIN PA3 + +#define HARDWARE_BIND_PLUG +// Hardware bind plug at PB2 (Pin 28) +#define BINDPLUG_PIN PB2 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART2 +#define RX_CHANNELS_TAER + +#define TELEMETRY_UART SERIAL_PORT_USART1 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USABLE_TIMER_CHANNEL_COUNT 13 +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) + diff --git a/src/main/target/ALIENFLIGHTNGF7/target.mk b/src/main/target/ALIENFLIGHTNGF7/target.mk new file mode 100644 index 0000000000..175dd29bc7 --- /dev/null +++ b/src/main/target/ALIENFLIGHTNGF7/target.mk @@ -0,0 +1,15 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += SDCARD VCP + +TARGET_SRC = \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_spi_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_spi_ms5611.c \ + drivers/compass_ak8963.c \ + drivers/compass_hmc5883l.c \ + drivers/compass_spi_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c From df14abc5c2284d6d03bd6cc62005e96b4b348074 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Feb 2017 06:29:55 +0000 Subject: [PATCH 15/27] Added PG config definitions 5 --- src/main/blackbox/blackbox.c | 17 ++++ src/main/fc/config.c | 19 ++-- src/main/flight/servos.c | 68 ++++++++++---- src/main/flight/servos.h | 2 +- src/main/io/ledstrip.c | 173 ++++++++++++++++------------------- 5 files changed, 159 insertions(+), 120 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index eb7cd7f810..fc6932899a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -66,6 +66,23 @@ #include "sensors/gyro.h" #include "sensors/sonar.h" +#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) +#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH +#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) +#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD +#else +#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SERIAL +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); + +PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, + .device = DEFAULT_BLACKBOX_DEVICE, + .rate_num = 1, + .rate_denom = 1, + .on_motor_test = 0 // default off +); + #define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define SLOW_FRAME_INTERVAL 4096 diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 1bdba3e79f..d613278249 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -235,8 +235,8 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) } #endif -#ifdef BARO #ifndef USE_PARAMETER_GROUPS +#ifdef BARO void resetBarometerConfig(barometerConfig_t *barometerConfig) { barometerConfig->baro_sample_count = 21; @@ -245,7 +245,6 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_cf_alt = 0.965f; } #endif -#endif #ifdef LED_STRIP void resetLedStripConfig(ledStripConfig_t *ledStripConfig) @@ -266,7 +265,9 @@ void resetLedStripConfig(ledStripConfig_t *ledStripConfig) ledStripConfig->ioTag = IO_TAG_NONE; } #endif +#endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_SERVOS void resetServoConfig(servoConfig_t *servoConfig) { @@ -285,7 +286,6 @@ void resetServoConfig(servoConfig_t *servoConfig) } #endif -#ifndef USE_PARAMETER_GROUPS void resetMotorConfig(motorConfig_t *motorConfig) { #ifdef BRUSHED_MOTORS @@ -961,21 +961,19 @@ void createDefaultConfig(master_t *config) config->airplaneConfig.fixedwing_althold_dir = 1; - // Motor/ESC/Servo #ifndef USE_PARAMETER_GROUPS + // Motor/ESC/Servo resetMixerConfig(&config->mixerConfig); resetMotorConfig(&config->motorConfig); -#endif #ifdef USE_SERVOS resetServoConfig(&config->servoConfig); #endif -#ifndef USE_PARAMETER_GROUPS resetFlight3DConfig(&config->flight3DConfig); -#endif #ifdef LED_STRIP resetLedStripConfig(&config->ledStripConfig); #endif +#endif #ifdef GPS // gps/nav stuff @@ -1030,6 +1028,7 @@ void createDefaultConfig(master_t *config) #endif #ifdef USE_SERVOS +#ifndef USE_PARAMETER_GROUPS // servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { config->servoProfile.servoConf[i].min = DEFAULT_SERVO_MIN; @@ -1040,6 +1039,7 @@ void createDefaultConfig(master_t *config) config->servoProfile.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; config->servoProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } +#endif // gimbal config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; @@ -1070,6 +1070,7 @@ void createDefaultConfig(master_t *config) memcpy(config->transponderConfig.data, &defaultTransponderData, sizeof(defaultTransponderData)); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef BLACKBOX #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) intFeatureSet(FEATURE_BLACKBOX, featuresPtr); @@ -1080,11 +1081,11 @@ void createDefaultConfig(master_t *config) #else config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL; #endif - config->blackboxConfig.rate_num = 1; config->blackboxConfig.rate_denom = 1; config->blackboxConfig.on_motor_test = 0; // default off #endif // BLACKBOX +#endif #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { @@ -1143,7 +1144,7 @@ void activateConfig(void) setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); #ifdef USE_SERVOS - servoUseConfigs(masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig); + servoUseConfigs(&masterConfig.channelForwardingConfig); #endif imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 73712a8799..9c2f60fe75 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -31,6 +31,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "drivers/pwm_output.h" #include "drivers/system.h" @@ -52,11 +53,47 @@ extern mixerMode_e currentMixerMode; +PG_REGISTER_WITH_RESET_FN(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0); + +void pgResetFn_servoConfig(servoConfig_t *servoConfig) +{ + servoConfig->dev.servoCenterPulse = 1500; + servoConfig->dev.servoPwmRate = 50; + servoConfig->tri_unarmed_servo = 1; + servoConfig->servo_lowpass_freq = 0; + + int servoIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) { + if (timerHardware[i].usageFlags & TIM_USE_SERVO) { + servoConfig->dev.ioTags[servoIndex] = timerHardware[i].tag; + servoIndex++; + } + } +} + +PG_REGISTER_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 0); + +PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 0); + +void pgResetFn_servoParams(servoParam_t *instance) +{ + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + RESET_CONFIG(const servoParam_t, &instance[i], + .min = DEFAULT_SERVO_MIN, + .max = DEFAULT_SERVO_MAX, + .middle = DEFAULT_SERVO_MIDDLE, + .rate = 100, + .angleAtMin = DEFAULT_SERVO_MIN_ANGLE, + .angleAtMax = DEFAULT_SERVO_MAX_ANGLE, + .forwardFromChannel = CHANNEL_FORWARDING_DISABLED + ); + } +} + static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; -static servoParam_t *servoConf; static channelForwardingConfig_t *channelForwardingConfig; @@ -147,27 +184,26 @@ const mixerRules_t servoMixers[] = { { 0, NULL }, }; -void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse) +void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse) { - servoConf = servoParamsToUse; channelForwardingConfig = channelForwardingConfigToUse; } int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) { - const uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; + const uint8_t channelToForwardFrom = servoParams(servoIndex)->forwardFromChannel; if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { return rcData[channelToForwardFrom]; } - return servoConf[servoIndex].middle; + return servoParams(servoIndex)->middle; } int servoDirection(int servoIndex, int inputSource) { // determine the direction (reversed or not) from the direction bitfield of the servo - if (servoConf[servoIndex].reversedSources & (1 << inputSource)) + if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) return -1; else return 1; @@ -386,7 +422,7 @@ STATIC_UNIT_TESTED void servoMixer(void) if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { uint8_t target = currentServoMixer[i].targetChannel; uint8_t from = currentServoMixer[i].inputSource; - uint16_t servo_width = servoConf[target].max - servoConf[target].min; + uint16_t servo_width = servoParams(target)->max - servoParams(target)->min; int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; @@ -406,7 +442,7 @@ STATIC_UNIT_TESTED void servoMixer(void) } for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; + servo[i] = ((int32_t)servoParams(i)->rate * servo[i]) / 100L; servo[i] += determineServoMiddleOrForwardFromChannel(i); } } @@ -430,8 +466,8 @@ static void servoTable(void) /* case MIXER_GIMBAL: - servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); - servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); + servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ @@ -447,18 +483,18 @@ static void servoTable(void) if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) { - servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 - (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; + servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 + (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; } else { - servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; - servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + servo[SERVO_GIMBAL_PITCH] += (int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch / 50; + servo[SERVO_GIMBAL_ROLL] += (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; } } } // constrain servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values + servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); // limit the values } } @@ -486,7 +522,7 @@ static void filterServos(void) servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); // Sanity check - servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); + servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max); } } #if defined(MIXER_DEBUG) diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 25db91ba70..f47df8b47b 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -126,7 +126,7 @@ bool isMixerUsingServos(void); void writeServos(void); void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); void loadCustomServoMixer(void); -void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse); +void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse); int servoDirection(int servoIndex, int fromChannel); void servoConfigureOutput(void); void servosInit(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 37e9899548..664ad6135b 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -27,24 +27,11 @@ #include "build/build_config.h" +#include "common/axis.h" #include "common/color.h" #include "common/maths.h" -#include "common/typeconversion.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#include "drivers/light_ws2811strip.h" -#include "drivers/system.h" -#include "drivers/serial.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/gpio.h" -#include "drivers/timer.h" -#include "drivers/rx_pwm.h" - #include "common/printf.h" -#include "common/axis.h" +#include "common/typeconversion.h" #include "common/utils.h" #include "config/config_profile.h" @@ -52,41 +39,39 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "drivers/light_ws2811strip.h" +#include "drivers/serial.h" +#include "drivers/system.h" + #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "sensors/battery.h" -#include "sensors/sensors.h" -#include "sensors/boardalignment.h" -#include "sensors/gyro.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" - -#include "io/ledstrip.h" -#include "io/beeper.h" -#include "io/motors.h" -#include "io/servos.h" -#include "io/gimbal.h" -#include "io/serial.h" -#include "io/gps.h" - #include "flight/failsafe.h" -#include "flight/mixer.h" -#include "flight/pid.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/navigation.h" +#include "flight/pid.h" +#include "flight/servos.h" + +#include "io/beeper.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/serial.h" #include "rx/rx.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" + #include "telemetry/telemetry.h" -/* -PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); -PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); -PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0); -PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0); -*/ +PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 0); static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -171,6 +156,57 @@ static const specialColorIndexes_t defaultSpecialColors[] = { }} }; +#ifndef USE_PARAMETER_GROUPS +void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) +{ + memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); +} + +void applyDefaultColors(hsvColor_t *colors) +{ + // copy hsv colors as default + memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *colors++ = hsv[colorIndex]; + } +} + +void applyDefaultModeColors(modeColorIndexes_t *modeColors) +{ + memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); +} + +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) +{ + memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} + +#else + +void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig) +{ + memset(ledStripConfig->ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); + // copy hsv colors as default + memset(ledStripConfig->colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); + BUILD_BUG_ON(LED_CONFIGURABLE_COLOR_COUNT < ARRAYLEN(hsv)); + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + ledStripConfig->colors[colorIndex] = hsv[colorIndex]; + } + memcpy_fn(&ledStripConfig->modeColors, &defaultModeColors, sizeof(defaultModeColors)); + memcpy_fn(&ledStripConfig->specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); + ledStripConfig->ledstrip_visual_beeper = 0; + ledStripConfig->ledstrip_aux_channel = THROTTLE; + + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if (timerHardware[i].usageFlags & TIM_USE_LED) { + ledStripConfig->ioTag = timerHardware[i].tag; + return; + } + } + ledStripConfig->ioTag = IO_TAG_NONE; +} +#endif + static int scaledThrottle; static int scaledAux; @@ -245,9 +281,9 @@ void reevaluateLedConfig(void) } // get specialColor by index -static hsvColor_t* getSC(ledSpecialColorIds_e index) +static const hsvColor_t* getSC(ledSpecialColorIds_e index) { - return &ledStripConfigMutable()->colors[ledStripConfig()->specialColors.color[index]]; + return &ledStripConfig()->colors[ledStripConfig()->specialColors.color[index]]; } static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; @@ -449,7 +485,7 @@ static void applyLedFixedLayers() case LED_FUNCTION_FLIGHT_MODE: for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]); + const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]); if (directionalColor) { color = *directionalColor; } @@ -578,7 +614,7 @@ static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer) *timer += timerDelayUs; if (!flash) { - hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); } } @@ -607,7 +643,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer) *timer += timerDelay; if (!flash) { - hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); } } @@ -1032,57 +1068,6 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) return true; } -/* -void pgResetFn_ledConfigs(ledConfig_t *instance) -{ - memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); -} - -void pgResetFn_colors(hsvColor_t *instance) -{ - // copy hsv colors as default - BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv)); - - for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { - *instance++ = hsv[colorIndex]; - } -} - -void pgResetFn_modeColors(modeColorIndexes_t *instance) -{ - memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors)); -} - -void pgResetFn_specialColors(specialColorIndexes_t *instance) -{ - memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors)); -} -*/ - -void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) -{ - memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); -} - -void applyDefaultColors(hsvColor_t *colors) -{ - // copy hsv colors as default - memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); - for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { - *colors++ = hsv[colorIndex]; - } -} - -void applyDefaultModeColors(modeColorIndexes_t *modeColors) -{ - memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); -} - -void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) -{ - memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); -} - void ledStripInit() { colors = ledStripConfigMutable()->colors; From 1425241d61b93093eff9a91af9f5635b726bbbff Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 26 Feb 2017 20:59:42 +1100 Subject: [PATCH 16/27] Removing duplicate bb device enum Changing SDCARD condition to that suggested by @mikeller Changed make sample to make official --- Makefile | 10 +++++----- src/main/blackbox/blackbox.c | 6 +++++- src/main/blackbox/blackbox.h | 15 ++++++++++----- src/main/blackbox/blackbox_io.h | 12 ------------ src/main/fc/fc_init.c | 4 ++-- 5 files changed, 22 insertions(+), 25 deletions(-) diff --git a/Makefile b/Makefile index 261f1de226..66dd7abbc4 100644 --- a/Makefile +++ b/Makefile @@ -101,9 +101,9 @@ HSE_VALUE ?= 8000000 # used for turning on features like VCP and SDCARD FEATURES = -SAMPLE_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 CC3D FURYF4 NAZE REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY -ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk))))) -OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS)) +OFFICIAL_TARGETS = ALIENFLIGHTF3 ALIENFLIGHTF4 ANYFCF7 BETAFLIGHTF3 BLUEJAYF4 CC3D FURYF4 NAZE REVO SIRINFPV SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY +ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk))))) +OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS)) VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk)) VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS))) @@ -1093,8 +1093,8 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S $(V1) echo "%% $(notdir $<)" "$(STDOUT)" $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< -## sample : Build all sample (travis) targets -sample: $(SAMPLE_TARGETS) +## official : Build all official (travis) targets +official: $(OFFICIAL_TARGETS) ## all : Build all valid targets all: $(VALID_TARGETS) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index eb7cd7f810..4a5ea193ce 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1657,8 +1657,12 @@ void handleBlackbox(timeUs_t currentTimeUs) static bool canUseBlackboxWithCurrentConfiguration(void) { +#ifdef USE_SDCARD return feature(FEATURE_BLACKBOX) && - (blackboxConfig()->device != BLACKBOX_SDCARD || feature(FEATURE_SDCARD)); + !(blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD && !feature(FEATURE_SDCARD)); +#else + return feature(FEATURE_BLACKBOX); +#endif } /** diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 2d374c673c..95db665747 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -23,11 +23,16 @@ #include "config/parameter_group.h" -typedef enum { - BLACKBOX_SERIAL = 0, - BLACKBOX_SPIFLASH, - BLACKBOX_SDCARD -} blackBoxDevice_e; +typedef enum BlackboxDevice { + BLACKBOX_DEVICE_SERIAL = 0, +#ifdef USE_FLASHFS + BLACKBOX_DEVICE_FLASH = 1, +#endif +#ifdef USE_SDCARD + BLACKBOX_DEVICE_SDCARD = 2, +#endif + +} BlackboxDevice_e; typedef struct blackboxConfig_s { uint8_t rate_num; diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 9dc7b7566b..f9347e833f 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -17,18 +17,6 @@ #pragma once -typedef enum BlackboxDevice { - BLACKBOX_DEVICE_SERIAL = 0, - -#ifdef USE_FLASHFS - BLACKBOX_DEVICE_FLASH = 1, -#endif -#ifdef USE_SDCARD - BLACKBOX_DEVICE_SDCARD = 2, -#endif - -} BlackboxDevice; - typedef enum { BLACKBOX_RESERVE_SUCCESS, BLACKBOX_RESERVE_TEMPORARY_FAILURE, diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index e5078d3706..6a44558f55 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -501,7 +501,7 @@ void init(void) #endif #ifdef USE_FLASHFS - if (blackboxConfig()->device == BLACKBOX_SPIFLASH) { + if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) { #if defined(USE_FLASH_M25P16) m25p16_init(flashConfig()); #endif @@ -510,7 +510,7 @@ void init(void) #endif #ifdef USE_SDCARD - if (feature(FEATURE_SDCARD) && blackboxConfig()->device == BLACKBOX_SDCARD) { + if (feature(FEATURE_SDCARD) && blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) { sdcardInsertionDetectInit(); sdcard_init(sdcardConfig()->useDma); afatfs_init(); From 9ad094f9aa792f3e1132c6390688cd2644677ddc Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 27 Feb 2017 00:20:52 +1300 Subject: [PATCH 17/27] Moved hardware specific parts into 'transponder_ir.c'. --- src/main/drivers/transponder_ir.c | 169 ++++++++++++++++++++ src/main/drivers/transponder_ir_stm32f30x.c | 139 ---------------- src/main/drivers/transponder_ir_stm32f4xx.c | 146 ----------------- src/main/target/OMNIBUS/target.mk | 1 - src/main/target/PIKOBLX/target.mk | 1 - src/main/target/RG_SSD_F3/target.mk | 1 - src/main/target/SPRACINGF3EVO/target.mk | 1 - src/main/target/SPRACINGF3MINI/target.mk | 1 - src/main/target/SPRACINGF3NEO/target.mk | 1 - 9 files changed, 169 insertions(+), 291 deletions(-) delete mode 100644 src/main/drivers/transponder_ir_stm32f30x.c delete mode 100644 src/main/drivers/transponder_ir_stm32f4xx.c diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 92c6ebcdc6..1b2ff16781 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -24,7 +24,12 @@ #include "dma.h" #include "nvic.h" #include "io.h" +#include "rcc.h" #include "timer.h" +#if defined(STM32F4) +#include "timer_stm32f4xx.h" +#endif + #include "transponder_ir.h" /* @@ -39,6 +44,135 @@ uint8_t transponderIrDMABuffer[TRANSPONDER_DMA_BUFFER_SIZE]; volatile uint8_t transponderIrDataTransferInProgress = 0; +static IO_t transponderIO = IO_NONE; +static TIM_TypeDef *timer = NULL; +#if defined(STM32F3) +static DMA_Channel_TypeDef *dmaChannel = NULL; +#elif defined(STM32F4) +static DMA_Stream_TypeDef *stream = NULL; +#else +#error "Transponder not supported on this MCU." +#endif + +static void TRANSPONDER_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) +{ + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + transponderIrDataTransferInProgress = 0; +#if defined(STM32F3) + DMA_Cmd(descriptor->channel, DISABLE); +#elif defined(STM32F4) + DMA_Cmd(descriptor->stream, DISABLE); +#endif + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); + } +} + +void transponderIrHardwareInit(ioTag_t ioTag) +{ + if (!ioTag) { + return; + } + + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_OCInitTypeDef TIM_OCInitStructure; + DMA_InitTypeDef DMA_InitStructure; + + const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); + timer = timerHardware->tim; + +#if defined(STM32F3) + if (timerHardware->dmaChannel == NULL) { + return; + } +#elif defined(STM32F4) + if (timerHardware->dmaStream == NULL) { + return; + } +#endif + + transponderIO = IOGetByTag(ioTag); + IOInit(transponderIO, OWNER_TRANSPONDER, 0); + IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); + + dmaInit(timerHardware->dmaIrqHandler, OWNER_TRANSPONDER, 0); + dmaSetHandler(timerHardware->dmaIrqHandler, TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, 0); + + RCC_ClockCmd(timerRCC(timer), ENABLE); + + /* Time base configuration */ + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = 156; + TIM_TimeBaseStructure.TIM_Prescaler = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); + + /* PWM1 Mode configuration: Channel1 */ + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + } + + TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_Pulse = 0; +#if defined(STM32F3) + TIM_OC1Init(timer, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); +#elif defined(STM32F4) + timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); + timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); +#endif + TIM_CtrlPWMOutputs(timer, ENABLE); + + /* configure DMA */ +#if defined(STM32F3) + dmaChannel = timerHardware->dmaChannel; + DMA_DeInit(dmaChannel); +#elif defined(STM32F4) + stream = timerHardware->dmaStream; + DMA_Cmd(stream, DISABLE); + DMA_DeInit(stream); +#endif + + DMA_StructInit(&DMA_InitStructure); + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); +#if defined(STM32F3) + DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)transponderIrDMABuffer; +#elif defined(STM32F4) + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)transponderIrDMABuffer; +#endif + DMA_InitStructure.DMA_BufferSize = TRANSPONDER_DMA_BUFFER_SIZE; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; +#if defined(STM32F3) + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; + DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; + + DMA_Init(dmaChannel, &DMA_InitStructure); +#elif defined(STM32F4) + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + + DMA_Init(stream, &DMA_InitStructure); +#endif + + TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); + +#if defined(STM32F3) + DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); +#elif defined(STM32F4) + DMA_ITConfig(stream, DMA_IT_TC, ENABLE); +#endif +} + bool transponderIrInit(void) { memset(&transponderIrDMABuffer, 0, TRANSPONDER_DMA_BUFFER_SIZE); @@ -57,6 +191,7 @@ bool transponderIrInit(void) transponderIrHardwareInit(ioTag); + return true; } @@ -119,6 +254,40 @@ void transponderIrUpdateData(const uint8_t* transponderData) updateTransponderDMABuffer(transponderData); } +void transponderIrDMAEnable(void) +{ +#if defined(STM32F3) + DMA_SetCurrDataCounter(dmaChannel, TRANSPONDER_DMA_BUFFER_SIZE); // load number of bytes to be transferred +#elif defined(STM32F4) + DMA_SetCurrDataCounter(stream, TRANSPONDER_DMA_BUFFER_SIZE); // load number of bytes to be transferred +#endif + TIM_SetCounter(timer, 0); + TIM_Cmd(timer, ENABLE); +#if defined(STM32F3) + DMA_Cmd(dmaChannel, ENABLE); +#elif defined(STM32F4) + DMA_Cmd(stream, ENABLE); +#endif +} + +void transponderIrDisable(void) +{ +#if defined(STM32F3) + DMA_Cmd(dmaChannel, DISABLE); +#elif defined(STM32F4) + DMA_Cmd(stream, DISABLE); +#endif + TIM_Cmd(timer, DISABLE); + + IOInit(transponderIO, OWNER_TRANSPONDER, 0); + IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); + +#ifdef TRANSPONDER_INVERTED + IOHi(transponderIO); +#else + IOLo(transponderIO); +#endif +} void transponderIrTransmit(void) { diff --git a/src/main/drivers/transponder_ir_stm32f30x.c b/src/main/drivers/transponder_ir_stm32f30x.c deleted file mode 100644 index 807d08016b..0000000000 --- a/src/main/drivers/transponder_ir_stm32f30x.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * 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 - -#include "io.h" -#include "nvic.h" - -#include "dma.h" -#include "rcc.h" -#include "timer.h" - -#include "transponder_ir.h" - -static IO_t transponderIO = IO_NONE; -static DMA_Channel_TypeDef *dmaChannel = NULL; -static TIM_TypeDef *timer = NULL; - -static void TRANSPONDER_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { - transponderIrDataTransferInProgress = 0; - DMA_Cmd(descriptor->channel, DISABLE); - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - } -} - - -void transponderIrHardwareInit(ioTag_t ioTag) -{ - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_OCInitTypeDef TIM_OCInitStructure; - DMA_InitTypeDef DMA_InitStructure; - - const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); - timer = timerHardware->tim; - - if (timerHardware->dmaChannel == NULL) { - return; - } - - transponderIO = IOGetByTag(ioTag); - IOInit(transponderIO, OWNER_TRANSPONDER, 0); - IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); - - dmaInit(timerHardware->dmaIrqHandler, OWNER_TRANSPONDER, 0); - dmaSetHandler(timerHardware->dmaIrqHandler, TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, 0); - RCC_ClockCmd(timerRCC(timer), ENABLE); - - /* Time base configuration */ - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 156; - TIM_TimeBaseStructure.TIM_Prescaler = 0; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); - - /* PWM1 Mode configuration: Channel1 */ - TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - } else { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - } - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; - TIM_OCInitStructure.TIM_Pulse = 0; - TIM_OC1Init(timer, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(timer, TIM_OCPreload_Enable); - - TIM_CtrlPWMOutputs(timer, ENABLE); - - /* configure DMA */ - dmaChannel = timerHardware->dmaChannel; - DMA_DeInit(dmaChannel); - - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)transponderIrDMABuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; - DMA_InitStructure.DMA_BufferSize = TRANSPONDER_DMA_BUFFER_SIZE; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - - DMA_Init(dmaChannel, &DMA_InitStructure); - - TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - - DMA_ITConfig(dmaChannel, DMA_IT_TC, ENABLE); - -} - -void transponderIrDMAEnable(void) -{ - DMA_SetCurrDataCounter(dmaChannel, TRANSPONDER_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(timer, 0); - TIM_Cmd(timer, ENABLE); - DMA_Cmd(dmaChannel, ENABLE); -} - -void transponderIrDisable(void) -{ - DMA_Cmd(dmaChannel, DISABLE); - TIM_Cmd(timer, DISABLE); - - IOInit(transponderIO, OWNER_TRANSPONDER, 0); - IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); - -#ifdef TRANSPONDER_INVERTED - IOHi(transponderIO); -#else - IOLo(transponderIO); -#endif -} - diff --git a/src/main/drivers/transponder_ir_stm32f4xx.c b/src/main/drivers/transponder_ir_stm32f4xx.c deleted file mode 100644 index fc89b5b540..0000000000 --- a/src/main/drivers/transponder_ir_stm32f4xx.c +++ /dev/null @@ -1,146 +0,0 @@ -/* - * 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 - -#include "io.h" -#include "nvic.h" - -#include "dma.h" -#include "rcc.h" -#include "timer.h" -#include "timer_stm32f4xx.h" - -#include "transponder_ir.h" - -static IO_t transponderIO = IO_NONE; -static DMA_Stream_TypeDef *stream = NULL; -static TIM_TypeDef *timer = NULL; - -static void TRANSPONDER_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { - transponderIrDataTransferInProgress = 0; - DMA_Cmd(descriptor->stream, DISABLE); - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - } -} - - -void transponderIrHardwareInit(ioTag_t ioTag) -{ - if (!ioTag) { - return; - } - - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_OCInitTypeDef TIM_OCInitStructure; - DMA_InitTypeDef DMA_InitStructure; - - const timerHardware_t *timerHardware = timerGetByTag(ioTag, TIM_USE_ANY); - timer = timerHardware->tim; - - if (timerHardware->dmaStream == NULL) { - return; - } - - transponderIO = IOGetByTag(ioTag); - IOInit(transponderIO, OWNER_TRANSPONDER, 0); - IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); - - dmaInit(timerHardware->dmaIrqHandler, OWNER_TRANSPONDER, 0); - dmaSetHandler(timerHardware->dmaIrqHandler, TRANSPONDER_DMA_IRQHandler, NVIC_PRIO_TRANSPONDER_DMA, 0); - - RCC_ClockCmd(timerRCC(timer), ENABLE); - - /* Time base configuration */ - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 156; - TIM_TimeBaseStructure.TIM_Prescaler = 0; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); - - /* PWM1 Mode configuration: Channel1 */ - TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - } else { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - } - - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; - TIM_OCInitStructure.TIM_Pulse = 0; - - timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); - timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); - - TIM_CtrlPWMOutputs(timer, ENABLE); - - /* configure DMA */ - stream = timerHardware->dmaStream; - DMA_Cmd(stream, DISABLE); - DMA_DeInit(stream); - - DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerCCR(timer, timerHardware->channel); - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)transponderIrDMABuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_BufferSize = TRANSPONDER_DMA_BUFFER_SIZE; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - - DMA_Init(stream, &DMA_InitStructure); - - TIM_DMACmd(timer, timerDmaSource(timerHardware->channel), ENABLE); - - DMA_ITConfig(stream, DMA_IT_TC, ENABLE); -} - -void transponderIrDMAEnable(void) -{ - DMA_SetCurrDataCounter(stream, TRANSPONDER_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(timer, 0); - TIM_Cmd(timer, ENABLE); - DMA_Cmd(stream, ENABLE); -} - -void transponderIrDisable(void) -{ - DMA_Cmd(stream, DISABLE); - TIM_Cmd(timer, DISABLE); - - IOInit(transponderIO, OWNER_TRANSPONDER, 0); - IOConfigGPIOAF(transponderIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_DOWN), timerHardware->alternateFunction); - -#ifdef TRANSPONDER_INVERTED - IOHi(transponderIO); -#else - IOLo(transponderIO); -#endif -} - diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 5878246a44..39f1a27259 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -12,5 +12,4 @@ TARGET_SRC = \ drivers/max7456.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c diff --git a/src/main/target/PIKOBLX/target.mk b/src/main/target/PIKOBLX/target.mk index 922b78405f..fe40d79e32 100644 --- a/src/main/target/PIKOBLX/target.mk +++ b/src/main/target/PIKOBLX/target.mk @@ -5,6 +5,5 @@ TARGET_SRC = \ drivers/accgyro_mpu.c \ drivers/accgyro_spi_mpu6000.c \ drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c diff --git a/src/main/target/RG_SSD_F3/target.mk b/src/main/target/RG_SSD_F3/target.mk index 06d9d1dfa3..9634576e0f 100644 --- a/src/main/target/RG_SSD_F3/target.mk +++ b/src/main/target/RG_SSD_F3/target.mk @@ -8,7 +8,6 @@ TARGET_SRC = \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c HSE_VALUE = 12000000 diff --git a/src/main/target/SPRACINGF3EVO/target.mk b/src/main/target/SPRACINGF3EVO/target.mk index 937cd6bf4d..527c06f149 100644 --- a/src/main/target/SPRACINGF3EVO/target.mk +++ b/src/main/target/SPRACINGF3EVO/target.mk @@ -9,6 +9,5 @@ TARGET_SRC = \ drivers/compass_ak8963.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c diff --git a/src/main/target/SPRACINGF3MINI/target.mk b/src/main/target/SPRACINGF3MINI/target.mk index a46cf1be8f..b1caf722bb 100644 --- a/src/main/target/SPRACINGF3MINI/target.mk +++ b/src/main/target/SPRACINGF3MINI/target.mk @@ -10,7 +10,6 @@ TARGET_SRC = \ drivers/compass_ak8963.c \ drivers/flash_m25p16.c \ drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ io/transponder_ir.c ifeq ($(TARGET), TINYBEEF3) diff --git a/src/main/target/SPRACINGF3NEO/target.mk b/src/main/target/SPRACINGF3NEO/target.mk index c596c191c3..5d664fa285 100755 --- a/src/main/target/SPRACINGF3NEO/target.mk +++ b/src/main/target/SPRACINGF3NEO/target.mk @@ -13,7 +13,6 @@ TARGET_SRC = \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ drivers/max7456.c \ drivers/vtx_rtc6705.c \ io/osd.c \ From ca67cf19909e9492905153ee052fca2182b5b2a1 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 19 Feb 2017 16:18:55 +1300 Subject: [PATCH 18/27] Added USB disconnect pulse for F4 / F7. --- Makefile | 6 ++- src/main/drivers/serial_usb_vcp.c | 6 +++ src/main/drivers/usb_detection.c | 55 ------------------------- src/main/drivers/usb_detection.h | 22 ---------- src/main/drivers/usb_io.c | 4 +- src/main/fc/fc_init.c | 2 +- src/main/target/BETAFLIGHTF3/target.h | 2 - src/main/target/COLIBRI_RACE/target.h | 2 - src/main/target/DOGE/target.h | 1 - src/main/target/FURYF3/target.h | 2 - src/main/target/IMPULSERCF3/target.h | 2 - src/main/target/LUX_RACE/target.h | 2 - src/main/target/OMNIBUS/target.h | 4 ++ src/main/target/RCEXPLORERF3/target.h | 2 - src/main/target/RG_SSD_F3/target.h | 2 - src/main/target/SIRINFPV/target.h | 2 - src/main/target/SPRACINGF3EVO/target.h | 2 - src/main/target/SPRACINGF3MINI/target.h | 5 +-- src/main/target/SPRACINGF3NEO/target.h | 2 - src/main/target/TINYFISH/target.h | 1 - 20 files changed, 18 insertions(+), 108 deletions(-) delete mode 100644 src/main/drivers/usb_detection.c delete mode 100644 src/main/drivers/usb_detection.h diff --git a/Makefile b/Makefile index 261f1de226..2b881c39f4 100644 --- a/Makefile +++ b/Makefile @@ -786,13 +786,15 @@ VCP_SRC = \ vcpf4/usbd_desc.c \ vcpf4/usbd_usr.c \ vcpf4/usbd_cdc_vcp.c \ - drivers/serial_usb_vcp.c + drivers/serial_usb_vcp.c \ + drivers/usb_io.c else ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS))) VCP_SRC = \ vcp_hal/usbd_desc.c \ vcp_hal/usbd_conf.c \ vcp_hal/usbd_cdc_interface.c \ - drivers/serial_usb_vcp.c + drivers/serial_usb_vcp.c \ + drivers/usb_io.c else VCP_SRC = \ vcp/hw_config.c \ diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 6014553cb4..97fdfe03c4 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -28,8 +28,10 @@ #if defined(STM32F4) #include "usb_core.h" #include "usbd_cdc_vcp.h" +#include "usb_io.h" #elif defined(STM32F7) #include "vcp_hal/usbd_cdc_interface.h" +#include "usb_io.h" USBD_HandleTypeDef USBD_Device; #else #include "usb_core.h" @@ -184,10 +186,14 @@ serialPort_t *usbVcpOpen(void) vcpPort_t *s; #if defined(STM32F4) + usbGenerateDisconnectPulse(); + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); #elif defined(STM32F7) + usbGenerateDisconnectPulse(); + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); /* Init Device Library */ diff --git a/src/main/drivers/usb_detection.c b/src/main/drivers/usb_detection.c deleted file mode 100644 index d1c0088c9e..0000000000 --- a/src/main/drivers/usb_detection.c +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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 "io.h" -#include "system.h" - -static IO_t usbDetectPin = IO_NONE; - -void usbCableDetectDeinit(void) -{ -#ifdef USB_DETECT_PIN - IOInit(usbDetectPin, OWNER_FREE, 0); - IOConfigGPIO(usbDetectPin, IOCFG_IN_FLOATING); - usbDetectPin = IO_NONE; -#endif -} - -void usbCableDetectInit(void) -{ -#ifdef USB_DETECT_PIN - usbDetectPin = IOGetByTag(IO_TAG(USB_DETECT_PIN)); - - IOInit(usbDetectPin, OWNER_USB_DETECT, 0); - IOConfigGPIO(usbDetectPin, IOCFG_OUT_PP); -#endif -} - -bool usbCableIsInserted(void) -{ - bool result = false; - -#ifdef USB_DETECT_PIN - result = IORead(usbDetectPin) != 0; -#endif - - return result; -} diff --git a/src/main/drivers/usb_detection.h b/src/main/drivers/usb_detection.h deleted file mode 100644 index 5a3c53d6b4..0000000000 --- a/src/main/drivers/usb_detection.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * 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 - -void usbCableDetectDeinit(void); -void usbCableDetectInit(void); -bool usbCableIsInserted(void); diff --git a/src/main/drivers/usb_io.c b/src/main/drivers/usb_io.c index 7d2c99fc89..8166dca4bf 100644 --- a/src/main/drivers/usb_io.c +++ b/src/main/drivers/usb_io.c @@ -20,7 +20,7 @@ #include "platform.h" -#ifdef USB_IO +#ifdef USE_VCP #include "io.h" #include "system.h" @@ -28,7 +28,6 @@ #include "sdcard.h" - #ifdef USB_DETECT_PIN static IO_t usbDetectPin = IO_NONE; #endif @@ -75,5 +74,4 @@ void usbGenerateDisconnectPulse(void) IOLo(usbPin); } - #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index e5078d3706..00bcefbd7c 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -488,7 +488,7 @@ void init(void) } #endif -#ifdef USB_CABLE_DETECTION +#ifdef USB_DETECT_PIN usbCableDetectInit(); #endif diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 04c906052f..6c27ce17b3 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -49,8 +49,6 @@ #define REMAP_TIM16_DMA #define REMAP_TIM17_DMA -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index ce232c883b..38e79644cd 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -75,8 +75,6 @@ #define USE_MAG_AK8963 #define USE_MAG_AK8975 -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 51e6d6ae0e..0dae0773f8 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -91,7 +91,6 @@ #define USE_BARO_BMP280 #define USE_BARO_SPI_BMP280 -#define USB_IO #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 24cc5df743..e9391ac55c 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -108,8 +108,6 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 19858114af..468fec4f3d 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -27,8 +27,6 @@ #define USABLE_TIMER_CHANNEL_COUNT 8 -#define USB_IO - #define USE_EXTI #define MPU_INT_EXTI PC13 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 3ea53966cc..03ef073164 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -113,8 +113,6 @@ #define ACC_MPU6500_ALIGN CW270_DEG #endif -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index d43a2f705a..732edb03e3 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -60,8 +60,12 @@ #define USB_IO #define USB_CABLE_DETECTION + #define USB_DETECT_PIN PB5 +#undef GPS + + #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index 3d9becdfaa..452d2bab52 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -58,8 +58,6 @@ #define SONAR_TRIGGER_PIN PA6 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) #define SONAR_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/RG_SSD_F3/target.h b/src/main/target/RG_SSD_F3/target.h index 4d600d3c41..2b45ae55e5 100644 --- a/src/main/target/RG_SSD_F3/target.h +++ b/src/main/target/RG_SSD_F3/target.h @@ -78,8 +78,6 @@ #define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC3 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 9ae3a3f2a3..362db90c83 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -53,8 +53,6 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 374eb1a66d..8ab7077f54 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -65,8 +65,6 @@ //#define SONAR -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 1f1712e0b2..c74eb41051 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -78,8 +78,6 @@ #define BRUSHED_ESC_AUTODETECT -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 @@ -107,12 +105,13 @@ #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) - #ifndef TINYBEEF3 #define SOFTSERIAL1_RX_PIN PA0 // PA0 / PAD3 #define SOFTSERIAL1_TX_PIN PA1 // PA1 / PAD4 #endif +#define USB_DETECT_PIN PB5 + #define SONAR_SOFTSERIAL1_EXCLUSIVE #define USE_SPI diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 4cf44851d0..4db3a63161 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -53,8 +53,6 @@ #define USE_MAG_AK8975 #define USE_MAG_HMC5883 -#define USB_IO - #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/TINYFISH/target.h b/src/main/target/TINYFISH/target.h index 294965d8a5..1deffa81bc 100644 --- a/src/main/target/TINYFISH/target.h +++ b/src/main/target/TINYFISH/target.h @@ -46,7 +46,6 @@ #if USB_VCP_ENABLED #define USE_VCP - #define USB_IO #define USBD_PRODUCT_STRING "tinyFISH" #define SERIAL_PORT_COUNT 6 #else From 447325ec2b023e8f9c4e344166b5fd40315894d3 Mon Sep 17 00:00:00 2001 From: Hydra Date: Mon, 20 Feb 2017 18:23:44 +0000 Subject: [PATCH 19/27] Use usbGenerateDisconnectPulse for F1/F3 targets. --- src/main/vcp/hw_config.c | 26 ++------------------------ 1 file changed, 2 insertions(+), 24 deletions(-) diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index f9ec7c32ee..508b15a0c7 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -37,6 +37,7 @@ #include #include "drivers/system.h" +#include "drivers/usb_io.h" #include "drivers/nvic.h" #include "common/utils.h" @@ -85,30 +86,7 @@ void Set_System(void) RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); #endif /* STM32L1XX_XD */ - /*Pull down PA12 to create USB Disconnect Pulse*/ // HJI -#if defined(STM32F303xC) // HJI - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); // HJI - - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; // HJI - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // HJI - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; // HJI - GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; // HJI - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; // HJI -#else - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // HJI - - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;// HJI - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;// HJI - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;// HJI -#endif - - GPIO_Init(GPIOA, &GPIO_InitStructure); // HJI - - GPIO_ResetBits(GPIOA, GPIO_Pin_12); // HJI - - delay(200); // HJI - - GPIO_SetBits(GPIOA, GPIO_Pin_12); // HJI + usbGenerateDisconnectPulse(); #if defined(STM32F37X) || defined(STM32F303xC) From e0e798a8e47260e59a508135e88653a83ed0fef1 Mon Sep 17 00:00:00 2001 From: Maxim Strinzha Date: Sun, 26 Feb 2017 21:16:50 +0300 Subject: [PATCH 20/27] LED strip issues #2501 --- src/main/drivers/light_ws2811strip.c | 2 +- src/main/io/ledstrip.c | 4 ++-- src/main/io/ledstrip.h | 1 - 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index ce6772f368..ccee17a4ed 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -87,7 +87,7 @@ void setStripColors(const hsvColor_t *colors) void ws2811LedStripInit(ioTag_t ioTag) { - memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); + memset(ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); ws2811LedStripHardwareInit(ioTag); const hsvColor_t hsv_white = { 0, 255, 255}; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 37e9899548..c9a83a0b28 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -798,7 +798,7 @@ static void applyLarsonScannerLayer(bool updateNow, timeUs_t *timer) int scannerLedIndex = 0; for (unsigned i = 0; i < ledCounts.count; i++) { - const ledConfig_t *ledConfig = &ledConfigs[i]; + const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) { hsvColor_t ledColor; @@ -827,7 +827,7 @@ static void applyLedBlinkLayer(bool updateNow, timeUs_t *timer) bool ledOn = (blinkMask & 1); // b_b_____... if (!ledOn) { for (int i = 0; i < ledCounts.count; ++i) { - const ledConfig_t *ledConfig = &ledConfigs[i]; + const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) || (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 50)) { diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 0cac24fc20..9dfbe4e403 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -150,7 +150,6 @@ typedef struct ledStripConfig_s { PG_DECLARE(ledStripConfig_t, ledStripConfig); -ledConfig_t *ledConfigs; hsvColor_t *colors; const modeColorIndexes_t *modeColors; specialColorIndexes_t specialColors; From 7ed1e1914ce3bb73bccd2673003474d74b42d644 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 22 Feb 2017 01:00:33 +1300 Subject: [PATCH 21/27] Re-add Spektrum bind pin for SPRACINGF3MINI / TINYBEEF3. --- src/main/target/SPRACINGF3MINI/target.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index c74eb41051..52f7822035 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -183,9 +183,12 @@ #define BUTTON_B_PIN PB0 #define HARDWARE_BIND_PLUG -#define BINDPLUG_PIN PB0 +#define BINDPLUG_PIN BUTTON_B_PIN #endif +#define SPEKTRUM_BIND +#define BIND_PIN UART2_RX_PIN + #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) From 964252514a72ac86b8efd19529a174caeb5422e8 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 27 Feb 2017 11:30:20 +1300 Subject: [PATCH 22/27] Fixed regression disabling GPS for OMNIBUS. --- src/main/target/OMNIBUS/target.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 732edb03e3..0a2fd6feba 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -63,9 +63,6 @@ #define USB_DETECT_PIN PB5 -#undef GPS - - #define USE_VCP #define USE_UART1 #define USE_UART2 From 6b27f10c944808dc24c9302068784466c112000a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Feb 2017 08:48:16 +0000 Subject: [PATCH 23/27] Added PG config definitions 6 --- Makefile | 1 + src/main/config/config_master.h | 1 + src/main/fc/cli.c | 2 +- src/main/fc/config.c | 25 ++--------- src/main/fc/config.h | 2 - src/main/fc/controlrate_profile.c | 73 +++++++++++++++++++++++++++++++ src/main/fc/controlrate_profile.h | 43 ++++++++++++++++++ src/main/fc/fc_core.c | 1 + src/main/fc/rc_controls.h | 15 ------- src/main/telemetry/smartport.c | 26 +++++------ 10 files changed, 137 insertions(+), 52 deletions(-) create mode 100644 src/main/fc/controlrate_profile.c create mode 100644 src/main/fc/controlrate_profile.h diff --git a/Makefile b/Makefile index 0801b7e222..266889dada 100644 --- a/Makefile +++ b/Makefile @@ -596,6 +596,7 @@ COMMON_SRC = \ drivers/system.c \ drivers/timer.c \ fc/config.c \ + fc/controlrate_profile.c \ fc/fc_init.c \ fc/fc_dispatch.c \ fc/fc_hardfaults.c \ diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 77ce577c69..655839a7f8 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -38,6 +38,7 @@ #include "drivers/serial.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/fc_core.h" diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b7a78c9915..71b79f3ff2 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -944,7 +944,7 @@ static systemConfig_t systemConfigCopy; #ifdef BEEPER static beeperDevConfig_t beeperDevConfigCopy; #endif -static controlRateConfig_t controlRateProfilesCopy[MAX_CONTROL_RATE_PROFILE_COUNT]; +static controlRateConfig_t controlRateProfilesCopy[CONTROL_RATE_PROFILE_COUNT]; static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; #endif // USE_PARAMETER_GROUPS diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2884b5c3ea..8f194d1f4c 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -127,8 +127,6 @@ PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONF master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; -controlRateConfig_t *currentControlRateProfile; - #ifndef USE_PARAMETER_GROUPS static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -148,7 +146,6 @@ static void resetCompassConfig(compassConfig_t* compassConfig) compassConfig->interruptTag = IO_TAG_NONE; #endif } -#endif static void resetControlRateProfile(controlRateConfig_t *controlRateConfig) { @@ -165,6 +162,7 @@ static void resetControlRateProfile(controlRateConfig_t *controlRateConfig) controlRateConfig->rates[axis] = 70; } } +#endif static void resetPidProfile(pidProfile_t *pidProfile) { @@ -768,14 +766,6 @@ uint8_t getCurrentControlRateProfileIndex(void) return systemConfigMutable()->activeRateProfile; } -static void setControlRateProfile(uint8_t controlRateProfileIndex) -{ - if (controlRateProfileIndex < MAX_CONTROL_RATE_PROFILE_COUNT) { - systemConfigMutable()->activeRateProfile = controlRateProfileIndex; - currentControlRateProfile = controlRateProfilesMutable(controlRateProfileIndex); - } -} - uint16_t getCurrentMinthrottle(void) { return motorConfig()->minthrottle; @@ -993,9 +983,11 @@ void createDefaultConfig(master_t *config) for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { resetProfile(&config->profile[ii]); } - for (int ii = 0; ii < MAX_CONTROL_RATE_PROFILE_COUNT; ++ii) { +#ifndef USE_PARAMETER_GROUPS + for (int ii = 0; ii < CONTROL_RATE_PROFILE_COUNT; ++ii) { resetControlRateProfile(&config->controlRateProfile[ii]); } +#endif config->compassConfig.mag_declination = 0; @@ -1363,15 +1355,6 @@ void changeProfile(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } -void changeControlRateProfile(uint8_t profileIndex) -{ - if (profileIndex >= MAX_RATEPROFILES) { - profileIndex = MAX_RATEPROFILES - 1; - } - setControlRateProfile(profileIndex); - generateThrottleCurve(); -} - void beeperOffSet(uint32_t mask) { beeperConfigMutable()->beeper_off_flags |= mask; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 2921d95005..164d06b4b5 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -89,8 +89,6 @@ PG_DECLARE(serialPinConfig_t, serialPinConfig); struct profile_s; extern struct profile_s *currentProfile; -struct controlRateConfig_s; -extern struct controlRateConfig_s *currentControlRateProfile; void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c new file mode 100644 index 0000000000..c89c9ea8e2 --- /dev/null +++ b/src/main/fc/controlrate_profile.c @@ -0,0 +1,73 @@ +/* + * 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 + +#include "platform.h" + +#include "common/axis.h" + +#include "config/config_reset.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/fc_rc.h" + +controlRateConfig_t *currentControlRateProfile; + + +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 0); + +void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) +{ + for (int i = 0; i < CONTROL_RATE_PROFILE_COUNT; i++) { + RESET_CONFIG(const controlRateConfig_t, &controlRateConfig[i], + .rcRate8 = 100, + .rcYawRate8 = 100, + .rcExpo8 = 0, + .thrMid8 = 50, + .thrExpo8 = 0, + .dynThrPID = 10, + .rcYawExpo8 = 0, + .tpa_breakpoint = 1650, + .rates[FD_ROLL] = 70, + .rates[FD_PITCH] = 70, + .rates[FD_YAW] = 70 + ); + } +} + +void setControlRateProfile(uint8_t controlRateProfileIndex) +{ + if (controlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT) { + systemConfigMutable()->activeRateProfile = controlRateProfileIndex; + currentControlRateProfile = controlRateProfilesMutable(controlRateProfileIndex); + } +} + +void changeControlRateProfile(uint8_t controlRateProfileIndex) +{ + if (controlRateProfileIndex >= CONTROL_RATE_PROFILE_COUNT) { + controlRateProfileIndex = CONTROL_RATE_PROFILE_COUNT - 1; + } + setControlRateProfile(controlRateProfileIndex); + generateThrottleCurve(); +} diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h new file mode 100644 index 0000000000..2911907077 --- /dev/null +++ b/src/main/fc/controlrate_profile.h @@ -0,0 +1,43 @@ +/* + * 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 + +#include + +#include "config/parameter_group.h" + + +typedef struct controlRateConfig_s { + uint8_t rcRate8; + uint8_t rcYawRate8; + uint8_t rcExpo8; + uint8_t thrMid8; + uint8_t thrExpo8; + uint8_t rates[3]; + uint8_t dynThrPID; + uint8_t rcYawExpo8; + uint16_t tpa_breakpoint; // Breakpoint where TPA is activated +} controlRateConfig_t; + +#define CONTROL_RATE_PROFILE_COUNT 3 +PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); + +extern controlRateConfig_t *currentControlRateProfile; + +void setControlRateProfile(uint8_t controlRateProfileIndex); +void changeControlRateProfile(uint8_t controlRateProfileIndex); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e4ce797d7c..70f80353a1 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -47,6 +47,7 @@ #include "fc/cli.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/fc_core.h" #include "fc/fc_rc.h" #include "fc/rc_adjustments.h" diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 19d649588f..45713ed1b2 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -151,21 +151,6 @@ typedef struct modeActivationProfile_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) -typedef struct controlRateConfig_s { - uint8_t rcRate8; - uint8_t rcYawRate8; - uint8_t rcExpo8; - uint8_t thrMid8; - uint8_t thrExpo8; - uint8_t rates[3]; - uint8_t dynThrPID; - uint8_t rcYawExpo8; - uint16_t tpa_breakpoint; // Breakpoint where TPA is activated -} controlRateConfig_t; - -#define MAX_CONTROL_RATE_PROFILE_COUNT 3 -PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); - extern int16_t rcCommand[4]; typedef struct rcControlsConfig_s { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 7698b4e385..ac7cc70a3d 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -21,21 +21,31 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "drivers/system.h" -#include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" +#include "drivers/sensor.h" +#include "drivers/system.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/fc_msp.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "fc/fc_msp.h" + +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/navigation.h" #include "io/beeper.h" #include "io/motors.h" #include "io/gps.h" #include "io/serial.h" +#include "msp/msp.h" + #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" @@ -44,23 +54,13 @@ #include "sensors/compass.h" #include "sensors/gyro.h" -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/mixer.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" -#include "flight/altitudehold.h" - #include "rx/rx.h" #include "rx/msp.h" #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "msp/msp.h" - extern profile_t *currentProfile; -extern controlRateConfig_t *currentControlRateProfile; enum { From 0f4a3028c55828cb6270f8f44ae84e3b2c49ede4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Feb 2017 14:34:16 +0000 Subject: [PATCH 24/27] Added PG config definitions 7 --- src/main/config/parameter_group_ids.h | 14 +++- src/main/fc/config.c | 108 +++++++++++++++++--------- src/main/flight/altitudehold.c | 6 ++ src/main/flight/mixer.c | 2 + src/main/flight/servos.c | 3 + src/main/io/beeper.c | 34 ++++++-- 6 files changed, 119 insertions(+), 48 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index b42d360939..8f62429de4 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -47,7 +47,7 @@ #define PG_MOTOR_3D_CONFIG 26 // Cleanflight has motor3DConfig_t, betaflight has flight3DConfig_t with more parameters #define PG_LED_STRIP_CONFIG 27 // structs OK #define PG_COLOR_CONFIG 28 // part of led strip, structs OK -#define PG_AIRPLANE_ALT_HOLD_CONFIG 29 // struct OK +#define PG_AIRPLANE_CONFIG 29 // struct OK #define PG_GPS_CONFIG 30 // struct OK #define PG_TELEMETRY_CONFIG 31 // betaflight has more and different data in telemetryConfig_t #define PG_FRSKY_TELEMETRY_CONFIG 32 // Cleanflight has split data out of PG_TELEMETRY_CONFIG @@ -84,9 +84,15 @@ #define PG_MODE_ACTIVATION_OPERATOR_CONFIG 500 #define PG_OSD_CONFIG 501 #define PG_BEEPER_CONFIG 502 -#define PG_PID_CONFIG 503 -#define PG_STATUS_LED_CONFIG 504 -#define PG_BETAFLIGHT_END 504 +#define PG_BEEPER_DEV_CONFIG 503 +#define PG_PID_CONFIG 504 +#define PG_STATUS_LED_CONFIG 505 +#define PG_FLASH_CONFIG 506 +#define PG_PPM_CONFIG 507 +#define PG_PWM_CONFIG 508 +#define PG_SERIAL_PIN_CONFIG 509 +#define PG_ADC_CONFIG 510 +#define PG_BETAFLIGHT_END 510 // OSD configuration (subject to change) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8f194d1f4c..291a7a6cd8 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -94,6 +94,9 @@ #include "telemetry/telemetry.h" +master_t masterConfig; // master config struct with data independent from profiles +profile_t *currentProfile; + #ifndef DEFAULT_FEATURES #define DEFAULT_FEATURES 0 #endif @@ -122,10 +125,24 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(pwmConfig_t, pwmConfig, PG_PWM_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(ppmConfig_t, ppmConfig, PG_PPM_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(statusLedConfig_t, statusLedConfig, PG_STATUS_LED_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(serialPinConfig_t, serialPinConfig, PG_SERIAL_PIN_CONFIG, 0); -master_t masterConfig; // master config struct with data independent from profiles -profile_t *currentProfile; +PG_REGISTER_WITH_RESET_TEMPLATE(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); +#ifdef USE_FLASHFS +#ifdef M25P16_CS_PIN +#define FLASH_CONFIG_CSTAG IO_TAG(M25P16_CS_PIN) +#else +#define FLASH_CONFIG_CSTAG IO_TAG_NONE +#endif + +PG_RESET_TEMPLATE(flashConfig_t, flashConfig, + .csTag = FLASH_CONFIG_CSTAG +); +#endif #ifndef USE_PARAMETER_GROUPS static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) @@ -344,7 +361,11 @@ void resetsdcardConfig(sdcardConfig_t *sdcardConfig) #endif #ifdef USE_ADC +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_adcConfig(adcConfig_t *adcConfig) +#else void resetAdcConfig(adcConfig_t *adcConfig) +#endif { #ifdef VBAT_ADC_PIN adcConfig->vbat.enabled = true; @@ -370,6 +391,7 @@ void resetAdcConfig(adcConfig_t *adcConfig) #endif +#ifndef USE_PARAMETER_GROUPS #ifdef BEEPER void resetBeeperDevConfig(beeperDevConfig_t *beeperDevConfig) { @@ -383,9 +405,14 @@ void resetBeeperDevConfig(beeperDevConfig_t *beeperDevConfig) beeperDevConfig->ioTag = IO_TAG(BEEPER); } #endif +#endif #if defined(USE_PWM) || defined(USE_PPM) +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_ppmConfig(ppmConfig_t *ppmConfig) +#else void resetPpmConfig(ppmConfig_t *ppmConfig) +#endif { #ifdef PPM_PIN ppmConfig->ioTag = IO_TAG(PPM_PIN); @@ -401,8 +428,13 @@ void resetPpmConfig(ppmConfig_t *ppmConfig) #endif } +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_pwmConfig(pwmConfig_t *pwmConfig) +#else void resetPwmConfig(pwmConfig_t *pwmConfig) +#endif { + pwmConfig->inputFilteringMode = INPUT_FILTERING_DISABLED; int inputIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && inputIndex < PWM_INPUT_PORT_COUNT; i++) { if (timerHardware[i].usageFlags & TIM_USE_PWM) { @@ -559,73 +591,77 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) # endif #endif -void resetSerialPinConfig(serialPinConfig_t *pSerialPinConfig) +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_serialPinConfig(serialPinConfig_t *serialPinConfig) +#else +void resetSerialPinConfig(serialPinConfig_t *serialPinConfig) +#endif { for (int port = 0 ; port < SERIAL_PORT_MAX_INDEX ; port++) { - pSerialPinConfig->ioTagRx[port] = IO_TAG(NONE); - pSerialPinConfig->ioTagTx[port] = IO_TAG(NONE); + serialPinConfig->ioTagRx[port] = IO_TAG(NONE); + serialPinConfig->ioTagTx[port] = IO_TAG(NONE); } for (int index = 0 ; index < SERIAL_PORT_COUNT ; index++) { switch (serialPortIdentifiers[index]) { case SERIAL_PORT_USART1: #ifdef USE_UART1 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART1)] = IO_TAG(UART1_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART1)] = IO_TAG(UART1_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART1)] = IO_TAG(UART1_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART1)] = IO_TAG(UART1_TX_PIN); #endif break; case SERIAL_PORT_USART2: #ifdef USE_UART2 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART2)] = IO_TAG(UART2_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART2)] = IO_TAG(UART2_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART2)] = IO_TAG(UART2_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART2)] = IO_TAG(UART2_TX_PIN); #endif break; case SERIAL_PORT_USART3: #ifdef USE_UART3 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART3)] = IO_TAG(UART3_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART3)] = IO_TAG(UART3_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART3)] = IO_TAG(UART3_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART3)] = IO_TAG(UART3_TX_PIN); #endif break; case SERIAL_PORT_USART4: #ifdef USE_UART4 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART4)] = IO_TAG(UART4_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART4)] = IO_TAG(UART4_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART4)] = IO_TAG(UART4_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART4)] = IO_TAG(UART4_TX_PIN); #endif break; case SERIAL_PORT_USART5: #ifdef USE_UART5 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART5)] = IO_TAG(UART5_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART5)] = IO_TAG(UART5_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART5)] = IO_TAG(UART5_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART5)] = IO_TAG(UART5_TX_PIN); #endif break; case SERIAL_PORT_USART6: #ifdef USE_UART6 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART6)] = IO_TAG(UART6_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART6)] = IO_TAG(UART6_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART6)] = IO_TAG(UART6_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART6)] = IO_TAG(UART6_TX_PIN); #endif break; case SERIAL_PORT_USART7: #ifdef USE_UART7 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART7)] = IO_TAG(UART7_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART7)] = IO_TAG(UART7_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART7)] = IO_TAG(UART7_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART7)] = IO_TAG(UART7_TX_PIN); #endif break; case SERIAL_PORT_USART8: #ifdef USE_UART8 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART8)] = IO_TAG(UART8_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART8)] = IO_TAG(UART8_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART8)] = IO_TAG(UART8_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_USART8)] = IO_TAG(UART8_TX_PIN); #endif break; case SERIAL_PORT_SOFTSERIAL1: #ifdef USE_SOFTSERIAL1 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL1)] = IO_TAG(SOFTSERIAL1_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL1)] = IO_TAG(SOFTSERIAL1_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL1)] = IO_TAG(SOFTSERIAL1_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL1)] = IO_TAG(SOFTSERIAL1_TX_PIN); #endif break; case SERIAL_PORT_SOFTSERIAL2: #ifdef USE_SOFTSERIAL2 - pSerialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL2)] = IO_TAG(SOFTSERIAL2_RX_PIN); - pSerialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL2)] = IO_TAG(SOFTSERIAL2_TX_PIN); + serialPinConfig->ioTagRx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL2)] = IO_TAG(SOFTSERIAL2_RX_PIN); + serialPinConfig->ioTagTx[SERIAL_PORT_IDENTIFIER_TO_RESOURCE_INDEX(SERIAL_PORT_SOFTSERIAL2)] = IO_TAG(SOFTSERIAL2_TX_PIN); #endif break; case SERIAL_PORT_USB_VCP: @@ -737,6 +773,7 @@ void resetStatusLedConfig(statusLedConfig_t *statusLedConfig) ; } +#ifndef USE_PARAMETER_GROUPS #ifdef USE_FLASHFS void resetFlashConfig(flashConfig_t *flashConfig) { @@ -747,6 +784,7 @@ void resetFlashConfig(flashConfig_t *flashConfig) #endif } #endif +#endif uint8_t getCurrentProfileIndex(void) { @@ -872,19 +910,21 @@ void createDefaultConfig(master_t *config) #ifndef USE_PARAMETER_GROUPS resetBatteryConfig(&config->batteryConfig); -#endif #if defined(USE_PWM) || defined(USE_PPM) resetPpmConfig(&config->ppmConfig); resetPwmConfig(&config->pwmConfig); #endif +#ifdef USE_PWM + config->pwmConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; +#endif -#ifndef USE_PARAMETER_GROUPS #ifdef TELEMETRY resetTelemetryConfig(&config->telemetryConfig); #endif #endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_ADC resetAdcConfig(&config->adcConfig); #endif @@ -892,6 +932,7 @@ void createDefaultConfig(master_t *config) #ifdef BEEPER resetBeeperDevConfig(&config->beeperDevConfig); #endif +#endif #ifdef SONAR resetSonarConfig(&config->sonarConfig); @@ -938,20 +979,13 @@ void createDefaultConfig(master_t *config) resetAllRxChannelRangeConfigurations(config->rxConfig.channelRanges); #endif -#ifdef USE_PWM - config->pwmConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; -#endif - #ifndef USE_PARAMETER_GROUPS config->armingConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support config->armingConfig.disarm_kill_switch = 1; config->armingConfig.auto_disarm_delay = 5; -#endif - config->airplaneConfig.fixedwing_althold_dir = 1; -#ifndef USE_PARAMETER_GROUPS // Motor/ESC/Servo resetMixerConfig(&config->mixerConfig); resetMotorConfig(&config->motorConfig); @@ -973,9 +1007,9 @@ void createDefaultConfig(master_t *config) config->gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif +#ifndef USE_PARAMETER_GROUPS resetSerialPinConfig(&config->serialPinConfig); -#ifndef USE_PARAMETER_GROUPS resetSerialConfig(&config->serialConfig); #endif @@ -1031,10 +1065,10 @@ void createDefaultConfig(master_t *config) config->servoProfile.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; config->servoProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } -#endif // gimbal config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; +#endif // Channel forwarding; config->channelForwardingConfig.startChannel = AUX1; @@ -1088,11 +1122,11 @@ void createDefaultConfig(master_t *config) } #endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_FLASHFS resetFlashConfig(&config->flashConfig); #endif -#ifndef USE_PARAMETER_GROUPS resetStatusLedConfig(&config->statusLedConfig); #endif diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 05edd90bb2..c6bd68aa14 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -44,6 +44,12 @@ #include "sensors/sonar.h" +PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); + +PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, + .fixedwing_althold_dir = 1 +); + int32_t setVelocity = 0; uint8_t velocityControl = 0; int32_t errorVelocityI = 0; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 0c20a275ec..b3a59d9318 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -107,6 +107,8 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) } } +PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); + #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9c2f60fe75..aff5615dac 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -90,6 +90,9 @@ void pgResetFn_servoParams(servoParam_t *instance) } } +// no template required since default is zero +PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0); + static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; int16_t servo[MAX_SUPPORTED_SERVOS]; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 6f0e0ce6ee..ba38d0fd6c 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -17,17 +17,22 @@ #include "stdbool.h" #include "stdint.h" -#include "stdlib.h" #include #include "common/utils.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sound_beeper.h" #include "drivers/system.h" -#include "sensors/battery.h" -#include "sensors/sensors.h" +#include "fc/config.h" +#include "fc/runtime_config.h" + +#include "io/beeper.h" #include "io/statusindicator.h" #include "io/vtx.h" @@ -35,12 +40,27 @@ #include "io/gps.h" #endif -#include "fc/config.h" -#include "fc/runtime_config.h" +#include "sensors/battery.h" +#include "sensors/sensors.h" -#include "config/feature.h" -#include "io/beeper.h" +PG_REGISTER_WITH_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, PG_BEEPER_DEV_CONFIG, 0); + +#ifdef BEEPER_INVERTED +#define IS_OPEN_DRAIN false +#define IS_INVERTED true +#else +#define IS_OPEN_DRAIN true +#define IS_INVERTED false +#endif +#ifndef BEEPER +#define BEEPER NONE +#endif +PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, + .isOpenDrain = IS_OPEN_DRAIN, + .isInverted = IS_INVERTED, + .ioTag = IO_TAG(BEEPER) +); #if FLASH_SIZE > 64 #define BEEPER_NAMES From 7ae57eb8cf3fd1b92e83c665667292f430a75cea Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Feb 2017 17:50:30 +0000 Subject: [PATCH 25/27] Added PG config definitions 8 --- src/main/cms/cms_menu_imu.c | 2 +- src/main/config/config_master.h | 2 +- src/main/fc/cli.c | 6 ++-- src/main/fc/config.c | 7 ++-- src/main/fc/config.h | 6 ---- src/main/fc/fc_msp.c | 2 +- src/main/flight/pid.c | 59 +++++++++++++++++++++++++++++++++ src/main/flight/pid.h | 6 ++++ src/main/io/dashboard.c | 1 + src/main/target/NAZE/config.c | 3 +- 10 files changed, 78 insertions(+), 16 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 534cac81b1..66d97cd23d 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -403,7 +403,7 @@ static OSD_Entry cmsx_menuImuEntries[] = {"MISC PP", OME_Submenu, cmsMenuChange, &cmsx_menuProfileOther, 0}, {"FILT PP", OME_Submenu, cmsMenuChange, &cmsx_menuFilterPerProfile, 0}, - {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, MAX_RATEPROFILES, 1}, 0}, + {"RATE PROF", OME_UINT8, cmsx_rateProfileIndexOnChange, &(OSD_UINT8_t){ &tmpRateProfileIndex, 1, CONTROL_RATE_PROFILE_COUNT, 1}, 0}, {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 655839a7f8..49cee1d845 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -309,7 +309,7 @@ typedef struct master_s { #endif profile_t profile[MAX_PROFILE_COUNT]; - controlRateConfig_t controlRateProfile[MAX_RATEPROFILES]; + controlRateConfig_t controlRateProfile[CONTROL_RATE_PROFILE_COUNT]; modeActivationProfile_t modeActivationProfile; adjustmentProfile_t adjustmentProfile; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 71b79f3ff2..531075dec9 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3422,7 +3422,7 @@ static void cliRateProfile(char *cmdline) return; } else { const int i = atoi(cmdline); - if (i >= 0 && i < MAX_RATEPROFILES) { + if (i >= 0 && i < CONTROL_RATE_PROFILE_COUNT) { changeControlRateProfile(i); cliRateProfile(""); } @@ -3451,7 +3451,7 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_ static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, const master_t *defaultConfig) { - if (rateProfileIndex >= MAX_RATEPROFILES) { + if (rateProfileIndex >= CONTROL_RATE_PROFILE_COUNT) { // Faulty values return; } @@ -4136,7 +4136,7 @@ static void printConfig(char *cmdline, bool doDiff) cliProfile(""); const uint8_t controlRateProfileIndexSave = getCurrentControlRateProfileIndex(); - for (uint32_t rateIndex = 0; rateIndex < MAX_RATEPROFILES; rateIndex++) { + for (uint32_t rateIndex = 0; rateIndex < CONTROL_RATE_PROFILE_COUNT; rateIndex++) { cliDumpRateProfile(rateIndex, dumpMask, &defaultConfig); } changeControlRateProfile(controlRateProfileIndexSave); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 291a7a6cd8..26576f6517 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -181,6 +181,7 @@ static void resetControlRateProfile(controlRateConfig_t *controlRateConfig) } #endif +#ifndef USE_PARAMETER_GROUPS static void resetPidProfile(pidProfile_t *pidProfile) { pidProfile->P8[ROLL] = 44; @@ -231,11 +232,14 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->itermThrottleThreshold = 350; pidProfile->itermAcceleratorGain = 1.0f; } +#endif +#ifndef USE_PARAMETER_GROUPS void resetProfile(profile_t *profile) { resetPidProfile(&profile->pidProfile); } +#endif #ifdef GPS void resetGpsProfile(gpsProfile_t *gpsProfile) @@ -1011,13 +1015,10 @@ void createDefaultConfig(master_t *config) resetSerialPinConfig(&config->serialPinConfig); resetSerialConfig(&config->serialConfig); -#endif - for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { resetProfile(&config->profile[ii]); } -#ifndef USE_PARAMETER_GROUPS for (int ii = 0; ii < CONTROL_RATE_PROFILE_COUNT; ++ii) { resetControlRateProfile(&config->controlRateProfile[ii]); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 164d06b4b5..602536bf64 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -30,12 +30,6 @@ #include "drivers/sound_beeper.h" #include "drivers/vcd.h" -#if FLASH_SIZE <= 128 -#define MAX_PROFILE_COUNT 2 -#else -#define MAX_PROFILE_COUNT 3 -#endif -#define MAX_RATEPROFILES 3 #define MAX_NAME_LENGTH 16 typedef enum { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 186bf13d57..8c73f00514 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1275,7 +1275,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } else { value = value & ~RATEPROFILE_MASK; - if (value >= MAX_RATEPROFILES) { + if (value >= CONTROL_RATE_PROFILE_COUNT) { value = 0; } changeControlRateProfile(value); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3a8b7c7222..6845c1fcd3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -30,6 +31,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "fc/fc_core.h" #include "fc/fc_rc.h" @@ -71,6 +73,63 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, .pid_process_denom = PID_PROCESS_DENOM_DEFAULT ); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 0); + +void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) +{ + for (int i = 0; i < MAX_PROFILE_COUNT; i++) { + RESET_CONFIG(const pidProfile_t, &pidProfiles[i], + .P8[ROLL] = 44, + .I8[ROLL] = 40, + .D8[ROLL] = 20, + .P8[PITCH] = 58, + .I8[PITCH] = 50, + .D8[PITCH] = 22, + .P8[YAW] = 70, + .I8[YAW] = 45, + .D8[YAW] = 20, + .P8[PIDALT] = 50, + .I8[PIDALT] = 0, + .D8[PIDALT] = 0, + .P8[PIDPOS] = 15, // POSHOLD_P * 100, + .I8[PIDPOS] = 0, // POSHOLD_I * 100, + .D8[PIDPOS] = 0, + .P8[PIDPOSR] = 34, // POSHOLD_RATE_P * 10, + .I8[PIDPOSR] = 14, // POSHOLD_RATE_I * 100, + .D8[PIDPOSR] = 53, // POSHOLD_RATE_D * 1000, + .P8[PIDNAVR] = 25, // NAV_P * 10, + .I8[PIDNAVR] = 33, // NAV_I * 100, + .D8[PIDNAVR] = 83, // NAV_D * 1000, + .P8[PIDLEVEL] = 50, + .I8[PIDLEVEL] = 50, + .D8[PIDLEVEL] = 100, + .P8[PIDMAG] = 40, + .P8[PIDVEL] = 55, + .I8[PIDVEL] = 55, + .D8[PIDVEL] = 75, + + .yaw_p_limit = YAW_P_LIMIT_MAX, + .pidSumLimit = PIDSUM_LIMIT, + .yaw_lpf_hz = 0, + .itermWindupPointPercent = 50, + .dterm_filter_type = FILTER_BIQUAD, + .dterm_lpf_hz = 100, // filtering ON by default + .dterm_notch_hz = 260, + .dterm_notch_cutoff = 160, + .vbatPidCompensation = 0, + .pidAtMinThrottle = PID_STABILISATION_ON, + .levelAngleLimit = 55, + .levelSensitivity = 55, + .setpointRelaxRatio = 20, + .dtermSetpointWeight = 100, + .yawRateAccelLimit = 10.0f, + .rateAccelLimit = 0.0f, + .itermThrottleThreshold = 350, + .itermAcceleratorGain = 1.0f + ); + } +} + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index dcc24be330..cd669a6214 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -87,6 +87,12 @@ typedef struct pidProfile_s { } pidProfile_t; //PG_DECLARE_PROFILE(pidProfile_t, pidProfile); +#if FLASH_SIZE <= 128 +#define MAX_PROFILE_COUNT 2 +#else +#define MAX_PROFILE_COUNT 3 +#endif +PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index f4302bfbba..297c6a911b 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -48,6 +48,7 @@ #include "config/parameter_group_ids.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 739938b866..83049fab89 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -26,6 +26,7 @@ #include "drivers/io.h" #include "fc/rc_controls.h" +#include "fc/controlrate_profile.h" #include "flight/failsafe.h" #include "flight/mixer.h" @@ -72,7 +73,7 @@ void targetConfiguration(master_t *config) config->profile[profileId].pidProfile.P8[PIDLEVEL] = 30; config->profile[profileId].pidProfile.D8[PIDLEVEL] = 30; - for (int rateProfileId = 0; rateProfileId < MAX_RATEPROFILES; rateProfileId++) { + for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { config->controlRateProfile[rateProfileId].rcRate8 = 100; config->controlRateProfile[rateProfileId].rcYawRate8 = 110; config->controlRateProfile[rateProfileId].rcExpo8 = 0; From 5e5fc09b1dc4ef1c0c3423c7bd1641e01b10dc27 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Feb 2017 17:50:30 +0000 Subject: [PATCH 26/27] Added PG config definitions 9 --- src/main/config/parameter_group_ids.h | 4 +++- src/main/fc/cli.c | 2 +- src/main/fc/config.c | 28 ++++++++++++++++++++------- src/main/flight/navigation.c | 17 ++++++++++++---- src/main/flight/pid.h | 1 - src/main/io/displayport_msp.c | 8 ++++++-- src/main/io/gps.c | 9 +++++++++ src/main/telemetry/ibus.c | 25 ++++++++++-------------- src/main/telemetry/ibus.h | 6 ------ 9 files changed, 63 insertions(+), 37 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 8f62429de4..d1f45b9c96 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -92,7 +92,9 @@ #define PG_PWM_CONFIG 508 #define PG_SERIAL_PIN_CONFIG 509 #define PG_ADC_CONFIG 510 -#define PG_BETAFLIGHT_END 510 +#define PG_SDCARD_CONFIG 511 +#define PG_DISPLAY_PORT_MSP_CONFIG 512 +#define PG_BETAFLIGHT_END 512 // OSD configuration (subject to change) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 531075dec9..197c646cc1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -660,7 +660,7 @@ static const clivalue_t valueTable[] = { { "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, { "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, #if defined(TELEMETRY_IBUS) - { "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, + { "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, #endif #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 26576f6517..69ef6cf92a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -142,6 +142,18 @@ PG_REGISTER_WITH_RESET_TEMPLATE(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0); PG_RESET_TEMPLATE(flashConfig_t, flashConfig, .csTag = FLASH_CONFIG_CSTAG ); +#endif // USE_FLASH_FS + +#ifdef USE_SDCARD +PG_REGISTER_WITH_RESET_TEMPLATE(sdcardConfig_t, sdcardConfig, PG_SDCARD_CONFIG, 0); +#if defined(SDCARD_DMA_CHANNEL_TX) +#define SDCARD_CONFIG_USE_DMA true +#else +#define SDCARD_CONFIG_USE_DMA false +#endif +PG_RESET_TEMPLATE(sdcardConfig_t, sdcardConfig, + .useDma = SDCARD_CONFIG_USE_DMA +); #endif #ifndef USE_PARAMETER_GROUPS @@ -239,7 +251,6 @@ void resetProfile(profile_t *profile) { resetPidProfile(&profile->pidProfile); } -#endif #ifdef GPS void resetGpsProfile(gpsProfile_t *gpsProfile) @@ -254,7 +265,6 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) } #endif -#ifndef USE_PARAMETER_GROUPS #ifdef BARO void resetBarometerConfig(barometerConfig_t *barometerConfig) { @@ -353,6 +363,7 @@ void resetSonarConfig(sonarConfig_t *sonarConfig) } #endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_SDCARD void resetsdcardConfig(sdcardConfig_t *sdcardConfig) { @@ -362,7 +373,8 @@ void resetsdcardConfig(sdcardConfig_t *sdcardConfig) sdcardConfig->useDma = false; #endif } -#endif +#endif // USE_SDCARD +#endif // USE_PARAMETER_GROUPS #ifdef USE_ADC #ifdef USE_PARAMETER_GROUPS @@ -392,7 +404,7 @@ void resetAdcConfig(adcConfig_t *adcConfig) #endif } -#endif +#endif // USE_ADC #ifndef USE_PARAMETER_GROUPS @@ -738,11 +750,13 @@ void resetMax7456Config(vcdProfile_t *pVcdProfile) } #endif +#ifndef USE_PARAMETER_GROUPS void resetDisplayPortProfile(displayPortProfile_t *pDisplayPortProfile) { pDisplayPortProfile->colAdjust = 0; pDisplayPortProfile->rowAdjust = 0; } +#endif // USE_PARAMETER_GROUPS #ifdef USE_PARAMETER_GROUPS void pgResetFn_statusLedConfig(statusLedConfig_t *statusLedConfig) @@ -942,12 +956,12 @@ void createDefaultConfig(master_t *config) resetSonarConfig(&config->sonarConfig); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_SDCARD intFeatureSet(FEATURE_SDCARD, featuresPtr); resetsdcardConfig(&config->sdcardConfig); #endif -#ifndef USE_PARAMETER_GROUPS #ifdef SERIALRX_PROVIDER config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else @@ -1001,7 +1015,6 @@ void createDefaultConfig(master_t *config) #ifdef LED_STRIP resetLedStripConfig(&config->ledStripConfig); #endif -#endif #ifdef GPS // gps/nav stuff @@ -1011,7 +1024,6 @@ void createDefaultConfig(master_t *config) config->gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif -#ifndef USE_PARAMETER_GROUPS resetSerialPinConfig(&config->serialPinConfig); resetSerialConfig(&config->serialConfig); @@ -1075,8 +1087,10 @@ void createDefaultConfig(master_t *config) config->channelForwardingConfig.startChannel = AUX1; #endif +#ifndef USE_PARAMETER_GROUPS #ifdef GPS resetGpsProfile(&config->gpsProfile); +#endif #endif // custom mixer. clear by defaults. diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 4b6650faa2..de7eeb990f 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -18,12 +18,12 @@ #include #include -#include -#include #include #include "platform.h" +#ifdef GPS + #include "build/debug.h" #include "common/axis.h" @@ -37,6 +37,7 @@ #include "drivers/system.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -55,9 +56,17 @@ #include "sensors/sensors.h" -extern int16_t magHold; +PG_REGISTER_WITH_RESET_TEMPLATE(gpsProfile_t, gpsProfile, PG_NAVIGATION_CONFIG, 0); -#ifdef GPS +PG_RESET_TEMPLATE(gpsProfile_t, gpsProfile, + .gps_wp_radius = 200, + .gps_lpf = 20, + .nav_slew_rate = 30, + .nav_controls_heading = 1, + .nav_speed_min = 100, + .nav_speed_max = 300, + .ap_mode = 40 +); bool areSticksInApModePosition(uint16_t ap_mode); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cd669a6214..1f47a5f987 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -86,7 +86,6 @@ typedef struct pidProfile_s { float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms } pidProfile_t; -//PG_DECLARE_PROFILE(pidProfile_t, pidProfile); #if FLASH_SIZE <= 128 #define MAX_PROFILE_COUNT 2 #else diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index ecf063f9e8..9c5d00fc46 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -26,17 +26,21 @@ #include "common/utils.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/display.h" #include "drivers/system.h" #include "fc/fc_msp.h" +#include "io/displayport_msp.h" + #include "msp/msp_protocol.h" #include "msp/msp_serial.h" -#include "io/displayport_msp.h" +// no template required since defaults are zero +PG_REGISTER(displayPortProfile_t, displayPortProfileMsp, PG_DISPLAY_PORT_MSP_CONFIG, 0); static displayPort_t mspDisplayPort; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 6c0f0fd10f..70b582a4a7 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -183,6 +183,15 @@ typedef enum { gpsData_t gpsData; +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); + +PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, + .provider = GPS_NMEA, + .sbasMode = SBAS_AUTO, + .autoConfig = GPS_AUTOCONFIG_ON, + .autoBaud = GPS_AUTOBAUD_OFF +); + static void shiftPacketLog(void) { uint32_t i; diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 42c8b40d85..04777a245e 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -24,35 +24,37 @@ #include #include -#include #include #include "platform.h" -#include "common/utils.h" #if defined(TELEMETRY) && defined(TELEMETRY_IBUS) +#include "common/axis.h" + +#include "common/utils.h" + #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "common/axis.h" - #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/serial.h" +#include "fc/rc_controls.h" + +#include "io/serial.h" + #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/barometer.h" -#include "io/serial.h" -#include "fc/rc_controls.h" #include "scheduler/scheduler.h" -#include "telemetry/telemetry.h" #include "telemetry/ibus.h" +#include "telemetry/telemetry.h" /* * iBus Telemetry is a half-duplex serial protocol. It shares 1 line for @@ -160,13 +162,6 @@ * */ -/* -PG_REGISTER_WITH_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig, PG_IBUS_TELEMETRY_CONFIG, 0); - -PG_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig, - .report_cell_voltage = false, - ); -*/ #define IBUS_TASK_PERIOD_US (1000) @@ -290,7 +285,7 @@ static void dispatchMeasurementReply(ibusAddress_t address) switch (sensorAddressTypeLookup[address - ibusBaseAddress]) { case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: value = getVbat() * 10; - if (ibusTelemetryConfig()->report_cell_voltage) { + if (telemetryConfig()->report_cell_voltage) { value /= batteryCellCount; } sendIbusMeasurement(address, value); diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h index 3688ea2ffc..a319e227d3 100644 --- a/src/main/telemetry/ibus.h +++ b/src/main/telemetry/ibus.h @@ -17,12 +17,6 @@ #pragma once -typedef struct ibusTelemetryConfig_s { - uint8_t report_cell_voltage; // report vbatt divided with cellcount -} ibusTelemetryConfig_t; - -PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig); - void initIbusTelemetry(void); void handleIbusTelemetry(void); From 7a7f7a7bb310d3fbf45eafd68fc0dbb8289592cf Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 27 Feb 2017 11:58:52 +0100 Subject: [PATCH 27/27] ALIENFLIGHT*. Fix compiler redefine warnings, --- src/main/target/ALIENFLIGHTF1/config.c | 4 ++++ src/main/target/ALIENFLIGHTF3/config.c | 4 ++++ src/main/target/ALIENFLIGHTF4/config.c | 4 ++++ src/main/target/ALIENFLIGHTNGF7/config.c | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index d5bb6bf671..03ee90dffd 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -35,6 +35,10 @@ #include "config/config_profile.h" #include "config/config_master.h" +#ifdef BRUSHED_MOTORS_PWM_RATE +#undef BRUSHED_MOTORS_PWM_RATE +#endif + #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index a9cff7bfb9..dfbf92461c 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -45,6 +45,10 @@ #include "hardware_revision.h" +#ifdef BRUSHED_MOTORS_PWM_RATE +#undef BRUSHED_MOTORS_PWM_RATE +#endif + #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 0f56688c2f..eba42556fc 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -53,6 +53,10 @@ #define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V #define CURRENTSCALE -667 // ACS712/714-30A - 66.666 mV/A inverted mode +#ifdef BRUSHED_MOTORS_PWM_RATE +#undef BRUSHED_MOTORS_PWM_RATE +#endif + #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index dcbda39554..153bc7d333 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -53,6 +53,10 @@ #define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V #define CURRENTSCALE -667 // ACS712/714-30A - 66.666 mV/A inverted mode +#ifdef BRUSHED_MOTORS_PWM_RATE +#undef BRUSHED_MOTORS_PWM_RATE +#endif + #define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz // alternative defaults settings for AlienFlight targets