From d32abc69f8af57f3ab3253cbd953165730f52cd4 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 26 Jul 2017 23:58:05 +0200 Subject: [PATCH 001/138] Copy profile to another profile (CLI, MSP and CMS) --- src/main/cms/cms_menu_imu.c | 69 +++++++++++++++++++++++++++++++ src/main/fc/cli.c | 40 ++++++++++++++++++ src/main/fc/controlrate_profile.c | 8 ++++ src/main/fc/controlrate_profile.h | 2 + src/main/fc/fc_msp.c | 12 ++++++ src/main/flight/pid.c | 8 ++++ src/main/flight/pid.h | 1 + src/main/msp/msp_protocol.h | 2 + 8 files changed, 142 insertions(+) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 1144618b54..26ebaa8c17 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -391,6 +391,73 @@ static CMS_Menu cmsx_menuFilterPerProfile = { .entries = cmsx_menuFilterPerProfileEntries, }; +static uint8_t cmsx_dstPidProfile; +static uint8_t cmsx_dstControlRateProfile; + +static const char * const cmsx_ProfileNames[] = { + "-", + "1", + "2", + "3" +}; + +static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames }; +static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames }; + +static long cmsx_menuCopyProfile_onEnter(void) +{ + cmsx_dstPidProfile = 0; + cmsx_dstControlRateProfile = 0; + + return 0; +} + +static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + if (cmsx_dstPidProfile > 0) { + copyPidProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex()); + } + + return 0; +} + +static long cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + if (cmsx_dstControlRateProfile > 0) { + copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex()); + } + + return 0; +} + +static OSD_Entry cmsx_menuCopyProfileEntries[] = +{ + { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0}, + + { "CPY PID PROF TO", OME_TAB, NULL, &cmsx_PidProfileTable, 0 }, + { "COPY PP", OME_Funcall, cmsx_CopyPidProfile, NULL, 0 }, + { "CPY RATE PROF TO", OME_TAB, NULL, &cmsx_ControlRateProfileTable, 0 }, + { "COPY RP", OME_Funcall, cmsx_CopyControlRateProfile, NULL, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuCopyProfile = { + .GUARD_text = "XCPY", + .GUARD_type = OME_MENU, + .onEnter = cmsx_menuCopyProfile_onEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuCopyProfileEntries, +}; + static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, @@ -404,6 +471,7 @@ static OSD_Entry cmsx_menuImuEntries[] = {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, + {"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} @@ -417,4 +485,5 @@ CMS_Menu cmsx_menuImu = { .onGlobalExit = NULL, .entries = cmsx_menuImuEntries, }; + #endif // CMS diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1245bdd6e9..fd795dcae3 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3266,6 +3266,44 @@ static void cliResource(char *cmdline) } #endif /* USE_RESOURCE_MGMT */ +static void cliCopyProfile(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliShowParseError(); + } else { + const int id = atoi(cmdline); // id = target profile id + if (id < 0 || id > MAX_PROFILE_COUNT-1) { + cliPrintLinef("Invalid profile id, choose [0-%d]", MAX_PROFILE_COUNT-1); + } else { + if (id == getCurrentPidProfileIndex()) { + cliPrintLine("Can not overwrite current selected profile"); + } else { + copyPidProfile(id, getCurrentPidProfileIndex()); + cliPrintLinef("Copied profile settings from %d to %d", getCurrentPidProfileIndex(), id); + } + } + } +} + +static void cliCopyRateProfile(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliShowParseError(); + } else { + const int id = atoi(cmdline); // id = target rateprofile id + if (id < 0 || id > CONTROL_RATE_PROFILE_COUNT-1) { + cliPrintLinef("Invalid rateprofile id, choose [0-%d]", CONTROL_RATE_PROFILE_COUNT-1); + } else { + if (id == getCurrentControlRateProfileIndex()) { + cliPrintLine("Can not overwrite current selected rateprofile"); + } else { + copyControlRateProfile(id, getCurrentControlRateProfileIndex()); + cliPrintLinef("Copied rateprofile settings from %d to %d", getCurrentControlRateProfileIndex(), id); + } + } + } +} + static void backupConfigs(void) { // make copies of configs to do differencing @@ -3564,6 +3602,8 @@ const clicmd_t cmdTable[] = { #ifdef VTX_CONTROL CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif + CLI_COMMAND_DEF("copyprofile", "copy current profile settings to another profile", " : target profile id", cliCopyProfile), + CLI_COMMAND_DEF("copyrateprofile", "copy current rateprofile settings to another rateprofile", " : target rate profile id", cliCopyRateProfile), }; static void cliHelp(char *cmdline) { diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index c89c9ea8e2..b8a4c65d52 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -71,3 +71,11 @@ void changeControlRateProfile(uint8_t controlRateProfileIndex) setControlRateProfile(controlRateProfileIndex); generateThrottleCurve(); } + +void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex) { + if ((dstControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1 && srcControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1) + && dstControlRateProfileIndex != srcControlRateProfileIndex + ) { + memcpy(controlRateProfilesMutable(dstControlRateProfileIndex), controlRateProfilesMutable(srcControlRateProfileIndex), sizeof(controlRateConfig_t)); + } +} diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 2911907077..2978d9110a 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -41,3 +41,5 @@ extern controlRateConfig_t *currentControlRateProfile; void setControlRateProfile(uint8_t controlRateProfileIndex); void changeControlRateProfile(uint8_t controlRateProfileIndex); + +void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6bebe242ea..8c6b3f61b9 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1299,6 +1299,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; + case MSP_COPY_PROFILE: + value = sbufReadU8(src); // 0 = pid profile, 1 = control rate profile + uint8_t dstProfileIndex = sbufReadU8(src); + uint8_t srcProfileIndex = sbufReadU8(src); + if (value == 0) { + copyPidProfile(dstProfileIndex, srcProfileIndex); + } + else if (value == 1) { + copyControlRateProfile(dstProfileIndex, srcProfileIndex); + } + break; + #if defined(GPS) || defined(MAG) case MSP_SET_HEADING: magHold = sbufReadU16(src); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 565be6bfd8..46c7f0add7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -482,3 +482,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } } + +void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex) { + if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1) + && dstPidProfileIndex != srcPidProfileIndex + ) { + memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t)); + } +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 70cce21129..0e5d7d1e8c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -133,5 +133,6 @@ void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile); +void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex); #endif diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 2529346f2a..6089d65c68 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -223,6 +223,8 @@ // External OSD displayport mode messages #define MSP_DISPLAYPORT 182 +#define MSP_COPY_PROFILE 183 + #define MSP_BEEPER_CONFIG 184 #define MSP_SET_BEEPER_CONFIG 185 From a74a5222bac119856fe51fac9231a6b61fc1008e Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Thu, 27 Jul 2017 22:11:11 +0200 Subject: [PATCH 002/138] Removed CLI implementation --- src/main/fc/cli.c | 40 ---------------------------------------- 1 file changed, 40 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index fd795dcae3..1245bdd6e9 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3266,44 +3266,6 @@ static void cliResource(char *cmdline) } #endif /* USE_RESOURCE_MGMT */ -static void cliCopyProfile(char *cmdline) -{ - if (isEmpty(cmdline)) { - cliShowParseError(); - } else { - const int id = atoi(cmdline); // id = target profile id - if (id < 0 || id > MAX_PROFILE_COUNT-1) { - cliPrintLinef("Invalid profile id, choose [0-%d]", MAX_PROFILE_COUNT-1); - } else { - if (id == getCurrentPidProfileIndex()) { - cliPrintLine("Can not overwrite current selected profile"); - } else { - copyPidProfile(id, getCurrentPidProfileIndex()); - cliPrintLinef("Copied profile settings from %d to %d", getCurrentPidProfileIndex(), id); - } - } - } -} - -static void cliCopyRateProfile(char *cmdline) -{ - if (isEmpty(cmdline)) { - cliShowParseError(); - } else { - const int id = atoi(cmdline); // id = target rateprofile id - if (id < 0 || id > CONTROL_RATE_PROFILE_COUNT-1) { - cliPrintLinef("Invalid rateprofile id, choose [0-%d]", CONTROL_RATE_PROFILE_COUNT-1); - } else { - if (id == getCurrentControlRateProfileIndex()) { - cliPrintLine("Can not overwrite current selected rateprofile"); - } else { - copyControlRateProfile(id, getCurrentControlRateProfileIndex()); - cliPrintLinef("Copied rateprofile settings from %d to %d", getCurrentControlRateProfileIndex(), id); - } - } - } -} - static void backupConfigs(void) { // make copies of configs to do differencing @@ -3602,8 +3564,6 @@ const clicmd_t cmdTable[] = { #ifdef VTX_CONTROL CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif - CLI_COMMAND_DEF("copyprofile", "copy current profile settings to another profile", " : target profile id", cliCopyProfile), - CLI_COMMAND_DEF("copyrateprofile", "copy current rateprofile settings to another rateprofile", " : target rate profile id", cliCopyRateProfile), }; static void cliHelp(char *cmdline) { From 04d08cb033b836beed3378871ebfdf0637a45e98 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sat, 29 Jul 2017 22:26:53 +0200 Subject: [PATCH 003/138] Made CMS code conditional --- src/main/cms/cms_menu_imu.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 26ebaa8c17..073e88617d 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -391,6 +391,8 @@ static CMS_Menu cmsx_menuFilterPerProfile = { .entries = cmsx_menuFilterPerProfileEntries, }; +#ifndef DISABLE_EXTENDED_CMS_MENUS + static uint8_t cmsx_dstPidProfile; static uint8_t cmsx_dstControlRateProfile; @@ -458,6 +460,8 @@ CMS_Menu cmsx_menuCopyProfile = { .entries = cmsx_menuCopyProfileEntries, }; +#endif + static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, @@ -471,7 +475,9 @@ static OSD_Entry cmsx_menuImuEntries[] = {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, +#ifndef DISABLE_EXTENDED_CMS_MENUS {"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0}, +#endif {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} From ad53b20f0871eb81bc6c5285fc1e25908fbc45e9 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 28 Aug 2017 21:45:01 +1200 Subject: [PATCH 004/138] Ordered commands list in CLI. --- src/main/fc/cli.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5abefb6d16..392e0dde7b 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3493,14 +3493,11 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif -#ifdef USE_RX_FRSKY_D - CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), -#endif + CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|profile|rates|all] {defaults}", cliDiff), #ifdef USE_DSHOT @@ -3522,6 +3519,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("flash_read", NULL, "
", cliFlashRead), CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), #endif +#ifdef USE_RX_FRSKY_D + CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), +#endif #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), #ifdef GPS From 58ae4d494a15c591ada0cc71a0a3abed7fcbdecd Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 29 Aug 2017 00:22:24 +0900 Subject: [PATCH 005/138] Avoid initializing TIM_USE_NONE pins --- src/main/drivers/timer.c | 3 +++ src/main/drivers/timer_hal.c | 3 +++ 2 files changed, 6 insertions(+) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index a53128bbdd..d51aa743eb 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -709,6 +709,9 @@ void timerInit(void) #if defined(STM32F3) || defined(STM32F4) for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; + if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { + continue; + } IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 95653b4517..4aeb43ca44 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -807,6 +807,9 @@ void timerInit(void) #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) for (int timerIndex = 0; timerIndex < USABLE_TIMER_CHANNEL_COUNT; timerIndex++) { const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; + if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { + continue; + } IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif From 76c4a1c8f0219afe6c34b9f54c604b8b30653272 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sun, 27 Aug 2017 21:14:07 +0300 Subject: [PATCH 006/138] Fix F7 digital pwm and ledstrip --- src/main/drivers/light_ws2811strip_hal.c | 16 +++++++--------- src/main/drivers/pwm_output_dshot_hal.c | 7 ++++++- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 4ae9ac2b9e..b4337a41d6 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -38,17 +38,15 @@ static TIM_HandleTypeDef TimHandle; static uint16_t timerChannel = 0; static bool timerNChannel = false; -void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) -{ - if (htim->Instance == TimHandle.Instance) { - //HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); - ws2811LedDataTransferInProgress = 0; - } -} - void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) { HAL_DMA_IRQHandler(TimHandle.hdma[descriptor->userParam]); + if(timerNChannel) { + HAL_TIMEx_PWMN_Stop_DMA(&TimHandle,timerChannel); + } else { + HAL_TIM_PWM_Stop_DMA(&TimHandle,timerChannel); + } + ws2811LedDataTransferInProgress = 0; } void ws2811LedStripHardwareInit(ioTag_t ioTag) @@ -86,7 +84,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) ws2811IO = IOGetByTag(ioTag); IOInit(ws2811IO, OWNER_LED_STRIP, 0); - IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index c4a65cfed4..bd3d6a9bb8 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -83,6 +83,11 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) { motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); + if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Stop_DMA(&motor->TimHandle,motor->timerHardware->channel); + } else { + HAL_TIM_PWM_Stop_DMA(&motor->TimHandle,motor->timerHardware->channel); + } } void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) @@ -96,7 +101,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m const uint8_t timerIndex = getTimerIndex(timer); IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); - IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); + IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); From 863b42c4ffeef40f29a4bfa49c8971f0c3802bbb Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 29 Aug 2017 11:02:10 +1200 Subject: [PATCH 007/138] Added TARGET_CONFIG for KAKUTEF4. --- src/main/target/KAKUTEF4/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 8c22321a1b..5c0c475cbe 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -20,6 +20,8 @@ #define USBD_PRODUCT_STRING "KakuteF4-V1" +#define TARGET_CONFIG + #define LED0_PIN PB5 #define LED1_PIN PB4 #define LED2_PIN PB6 From c103e3074e76090c2f70fcfa8df8bdaa090e66b9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 29 Aug 2017 05:38:20 +0100 Subject: [PATCH 008/138] Fix gyro debug raw initialisation --- src/main/sensors/gyro.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4261c2307a..a2f89ae770 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -362,6 +362,7 @@ bool gyroInit(void) case DEBUG_FFT: case DEBUG_GYRO_NOTCH: case DEBUG_GYRO: + case DEBUG_GYRO_RAW: gyroDebugMode = debugMode; default: // debugMode is not gyro-related From f7c66869e8b379c2b3c5a353a4009b348de6c912 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 28 Aug 2017 23:33:28 +1200 Subject: [PATCH 009/138] Added support for decoding BLHeli32 ESC info. --- src/main/drivers/pwm_output.h | 8 ++ src/main/fc/cli.c | 173 +++++++++++++++++++++++++--------- 2 files changed, 137 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 6e3e8c9065..c95590226f 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -53,6 +53,14 @@ typedef enum { DSHOT_CMD_SAVE_SETTINGS, DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, + DSHOT_CMD_LED0_ON, // BLHeli32 only + DSHOT_CMD_LED1_ON, // BLHeli32 only + DSHOT_CMD_LED2_ON, // BLHeli32 only + DSHOT_CMD_LED3_ON, // BLHeli32 only + DSHOT_CMD_LED0_OFF, // BLHeli32 only + DSHOT_CMD_LED1_OFF, // BLHeli32 only + DSHOT_CMD_LED2_OFF, // BLHeli32 only + DSHOT_CMD_LED3_OFF, // BLHeli32 only DSHOT_CMD_MAX = 47 } dshotCommands_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5abefb6d16..bff82ad570 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2253,64 +2253,98 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) { #ifdef USE_DSHOT -#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15 -#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21 +#define ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE 15 +#define ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE 21 +#define ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE 64 + +enum { + ESC_INFO_KISS_V1, + ESC_INFO_KISS_V2, + ESC_INFO_BLHELI32 +}; #define ESC_INFO_VERSION_POSITION 12 -void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) +void printEscInfo(const uint8_t *escInfoBuffer, uint8_t bytesRead) { bool escInfoReceived = false; if (bytesRead > ESC_INFO_VERSION_POSITION) { - uint8_t escInfoVersion = 0; - uint8_t frameLength = 0; - if (escInfoBytes[ESC_INFO_VERSION_POSITION] == 255) { - escInfoVersion = 2; - frameLength = ESC_INFO_V2_EXPECTED_FRAME_SIZE; + uint8_t escInfoVersion; + uint8_t frameLength; + if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 254) { + escInfoVersion = ESC_INFO_BLHELI32; + frameLength = ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE; + } else if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 255) { + escInfoVersion = ESC_INFO_KISS_V2; + frameLength = ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE; } else { - escInfoVersion = 1; - frameLength = ESC_INFO_V1_EXPECTED_FRAME_SIZE; + escInfoVersion = ESC_INFO_KISS_V1; + frameLength = ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE; } - if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE)) - || ((escInfoVersion == 2) && (bytesRead == ESC_INFO_V2_EXPECTED_FRAME_SIZE))) { + if (bytesRead == frameLength) { escInfoReceived = true; - if (calculateCrc8(escInfoBytes, frameLength - 1) == escInfoBytes[frameLength - 1]) { + if (calculateCrc8(escInfoBuffer, frameLength - 1) == escInfoBuffer[frameLength - 1]) { uint8_t firmwareVersion = 0; - char firmwareSubVersion = 0; + uint8_t firmwareSubVersion = 0; uint8_t escType = 0; switch (escInfoVersion) { - case 1: - firmwareVersion = escInfoBytes[12]; - firmwareSubVersion = (char)((escInfoBytes[13] & 0x1f) + 97); - escType = (escInfoBytes[13] & 0xe0) >> 5; + case ESC_INFO_KISS_V1: + firmwareVersion = escInfoBuffer[12]; + firmwareSubVersion = (escInfoBuffer[13] & 0x1f) + 97; + escType = (escInfoBuffer[13] & 0xe0) >> 5; break; - case 2: - firmwareVersion = escInfoBytes[13]; - firmwareSubVersion = (char)escInfoBytes[14]; - escType = escInfoBytes[15]; + case ESC_INFO_KISS_V2: + firmwareVersion = escInfoBuffer[13]; + firmwareSubVersion = escInfoBuffer[14]; + escType = escInfoBuffer[15]; + + break; + case ESC_INFO_BLHELI32: + firmwareVersion = escInfoBuffer[13]; + firmwareSubVersion = escInfoBuffer[14]; + escType = escInfoBuffer[15]; break; } cliPrint("ESC: "); - switch (escType) { - case 1: - cliPrintLine("KISS8A"); + switch (escInfoVersion) { + case ESC_INFO_KISS_V1: + case ESC_INFO_KISS_V2: + switch (escType) { + case 1: + cliPrintLine("KISS8A"); + + break; + case 2: + cliPrintLine("KISS16A"); + + break; + case 3: + cliPrintLine("KISS24A"); + + break; + case 5: + cliPrintLine("KISS Ultralite"); + + break; + default: + cliPrintLine("unknown"); + + break; + } + break; - case 2: - cliPrintLine("KISS16A"); - break; - case 3: - cliPrintLine("KISS24A"); - break; - case 5: - cliPrintLine("KISS Ultralite"); - break; - default: - cliPrintLine("unknown"); + case ESC_INFO_BLHELI32: + { + char *escType = (char *)(escInfoBuffer + 31); + escType[32] = 0; + cliPrintLine(escType); + } + break; } @@ -2319,14 +2353,64 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) if (i && (i % 3 == 0)) { cliPrint("-"); } - cliPrintf("%02x", escInfoBytes[i]); + cliPrintf("%02x", escInfoBuffer[i]); } cliPrintLinefeed(); - cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, firmwareSubVersion); - if (escInfoVersion == 2) { - cliPrintLinef("Rotation: %s", escInfoBytes[16] ? "reversed" : "normal"); - cliPrintLinef("3D: %s", escInfoBytes[17] ? "on" : "off"); + switch (escInfoVersion) { + case ESC_INFO_KISS_V1: + case ESC_INFO_KISS_V2: + cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, (char)firmwareSubVersion); + + break; + case ESC_INFO_BLHELI32: + cliPrintLinef("Firmware: %d.%02d%", firmwareVersion, firmwareSubVersion); + + break; + } + if (escInfoVersion == ESC_INFO_KISS_V2 || escInfoVersion == ESC_INFO_BLHELI32) { + cliPrintLinef("Rotation: %s", escInfoBuffer[16] ? "reversed" : "normal"); + cliPrintLinef("3D: %s", escInfoBuffer[17] ? "on" : "off"); + if (escInfoVersion == ESC_INFO_BLHELI32) { + uint8_t setting = escInfoBuffer[18]; + cliPrint("Low voltage limit: "); + switch (setting) { + case 0: + cliPrintLine("off"); + + break; + case 255: + cliPrintLine("unsupported"); + + break; + default: + cliPrintLinef("%d.%01d", setting / 10, setting % 10); + + break; + } + + setting = escInfoBuffer[19]; + cliPrint("Current limit: "); + switch (setting) { + case 0: + cliPrintLine("off"); + + break; + case 255: + cliPrintLine("unsupported"); + + break; + default: + cliPrintLinef("%d", setting); + + break; + } + + for (int i = 0; i < 4; i++) { + setting = escInfoBuffer[i + 20]; + cliPrintLinef("LED %d: %s", i, setting ? (setting == 255) ? "unsupported" : "on" : "off"); + } + } } } else { cliPrintLine("Checksum error."); @@ -2341,14 +2425,15 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) static void executeEscInfoCommand(uint8_t escIndex) { - uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE]; cliPrintLinef("Info for ESC %d:", escIndex); - startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE); + uint8_t escInfoBuffer[ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE]; + + startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE); pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO); - delay(5); + delay(10); printEscInfo(escInfoBuffer, getNumberEscBytesRead()); } From 856860f7d47b0bdea3414f8693f868b7e0cdb0cb Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 29 Aug 2017 21:38:19 +1200 Subject: [PATCH 010/138] Increased EEPROM config version after 3.2.0-RC4 release. --- src/main/config/config_eeprom.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 2994fb4ba4..408a8e11aa 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -20,7 +20,7 @@ #include #include -#define EEPROM_CONF_VERSION 161 +#define EEPROM_CONF_VERSION 162 bool isEEPROMContentValid(void); bool loadEEPROM(void); From 8c3d8c3a46f016ec7413794ee1f18ec5097d3c29 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 30 Aug 2017 00:59:43 +0900 Subject: [PATCH 011/138] Remove reference to TIMER_OUTPUT_ENABLED --- src/main/drivers/serial_escserial.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 07bb0b81f7..31bd3b7205 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -944,7 +944,7 @@ void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uin else { uint8_t first_output = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - if (timerHardware[i].output & TIMER_OUTPUT_ENABLED) { + if (timerHardware[i].usageFlags & TIM_USE_MOTOR) { first_output = i; break; } From eadaff0bdd3b34790a5f4d4ac81eb81e6794534c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 30 Aug 2017 08:50:40 +0100 Subject: [PATCH 012/138] Code tidy and comments update --- src/main/drivers/accgyro/accgyro_mpu.c | 20 +++++----- src/main/fc/fc_rc.c | 53 +++++++++++++------------- src/main/flight/pid.c | 7 ++-- 3 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index d15d4aa79a..c7a10665bf 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -39,16 +39,16 @@ #include "drivers/system.h" #include "drivers/time.h" -#include "accgyro.h" -#include "accgyro_mpu3050.h" -#include "accgyro_mpu6050.h" -#include "accgyro_mpu6500.h" -#include "accgyro_spi_bmi160.h" -#include "accgyro_spi_icm20689.h" -#include "accgyro_spi_mpu6000.h" -#include "accgyro_spi_mpu6500.h" -#include "accgyro_spi_mpu9250.h" -#include "accgyro_mpu.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu3050.h" +#include "drivers/accgyro/accgyro_mpu6050.h" +#include "drivers/accgyro/accgyro_mpu6500.h" +#include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20689.h" +#include "drivers/accgyro/accgyro_spi_mpu6000.h" +#include "drivers/accgyro/accgyro_spi_mpu6500.h" +#include "drivers/accgyro/accgyro_spi_mpu9250.h" +#include "drivers/accgyro/accgyro_mpu.h" mpuResetFnPtr mpuResetFn; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index b620aefd16..f0ac7e669b 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -23,14 +23,11 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" #include "common/utils.h" -#include "common/filter.h" #include "config/feature.h" -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -40,33 +37,36 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" #include "scheduler/scheduler.h" #include "sensors/battery.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/pid.h" -#include "flight/mixer.h" static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; -float getSetpointRate(int axis) { +float getSetpointRate(int axis) +{ return setpointRate[axis]; } -float getRcDeflection(int axis) { +float getRcDeflection(int axis) +{ return rcDeflection[axis]; } -float getRcDeflectionAbs(int axis) { +float getRcDeflectionAbs(int axis) +{ return rcDeflectionAbs[axis]; } -float getThrottlePIDAttenuation(void) { +float getThrottlePIDAttenuation(void) +{ return throttlePIDAttenuation; } @@ -75,10 +75,8 @@ static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for void generateThrottleCurve(void) { - uint8_t i; - - for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; + for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { + const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - currentControlRateProfile->thrMid8; @@ -89,7 +87,7 @@ void generateThrottleCurve(void) } } -int16_t rcLookupThrottle(int32_t tmp) +static int16_t rcLookupThrottle(int32_t tmp) { const int32_t tmp2 = tmp / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] @@ -114,6 +112,7 @@ static void calculateSetpointRate(int axis) rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); } + // scale rcCommandf to range [-1.0, 1.0] float rcCommandf = rcCommand[axis] / 500.0f; rcDeflection[axis] = rcCommandf; const float rcCommandfAbs = ABS(rcCommandf); @@ -196,16 +195,16 @@ void processRcCommand(void) if (rxConfig()->rcInterpolation) { // Set RC refresh rate for sampling and channels to filter switch (rxConfig()->rcInterpolation) { - case(RC_SMOOTHING_AUTO): - rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps - break; - case(RC_SMOOTHING_MANUAL): - rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; - break; - case(RC_SMOOTHING_OFF): - case(RC_SMOOTHING_DEFAULT): - default: - rxRefreshRate = rxGetRefreshRate(); + case RC_SMOOTHING_AUTO: + rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps + break; + case RC_SMOOTHING_MANUAL: + rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; + break; + case RC_SMOOTHING_OFF: + case RC_SMOOTHING_DEFAULT: + default: + rxRefreshRate = rxGetRefreshRate(); } if (isRXDataNew && rxRefreshRate > 0) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 064a2c95ac..d723d0041d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -353,6 +353,7 @@ static float calcHorizonLevelStrength(void) static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination + // rcDeflection is in range [-1.0, 1.0] float angle = pidProfile->levelSensitivity * getRcDeflection(axis); #ifdef GPS angle += GPS_angle[axis]; @@ -360,11 +361,11 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit); const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed + // ANGLE mode - control is angle based currentPidSetpoint = errorAngle * levelGain; } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel + // HORIZON mode - mix of ANGLE and ACRO modes + // mix in errorAngle to currentPidSetpoint to add a little auto-level feel const float horizonLevelStrength = calcHorizonLevelStrength(); currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength); } From 1eee37221802ff6aa850b74e5d45f5965cd03440 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Wed, 30 Aug 2017 04:03:39 -0400 Subject: [PATCH 013/138] * Created ALIENWHOOP (ALIENWHOOPF4 and ALIENWHOOPF7) target specific PIDs, rates, and other settings for optimal performance in config.c --- src/main/target/ALIENWHOOP/config.c | 77 ++++++++++++++++++++++++++--- 1 file changed, 70 insertions(+), 7 deletions(-) diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index 905b61c268..c52de76d88 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -39,16 +39,22 @@ #ifdef TARGET_CONFIG +#include "fc/rc_modes.h" #include "common/axis.h" #include "config/feature.h" #include "drivers/pwm_esc_detect.h" +#include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/rc_controls.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "io/beeper.h" #include "io/serial.h" #include "rx/rx.h" #include "sensors/barometer.h" #include "sensors/boardalignment.h" #include "sensors/compass.h" +#include "sensors/gyro.h" #ifdef BRUSHED_MOTORS_PWM_RATE #undef BRUSHED_MOTORS_PWM_RATE @@ -59,27 +65,84 @@ void targetConfiguration(void) { - if (hardwareMotorType == MOTOR_BRUSHED) { + if (hardwareMotorType == MOTOR_BRUSHED) + { motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfigMutable()->minthrottle = 1080; - motorConfigMutable()->maxthrottle = 2000; - pidConfigMutable()->pid_process_denom = 1; + motorConfigMutable()->minthrottle = 1080; + motorConfigMutable()->maxthrottle = 2000; } - rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + /* Default to Spektrum */ + rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048; + rxConfigMutable()->spektrum_sat_bind = 5; + rxConfigMutable()->spektrum_sat_bind_autoreset = 1; + parseRcChannels("TAER1234", rxConfigMutable()); #if defined(ALIENWHOOPF4) rxConfigMutable()->sbus_inversion = 0; // TODO: what to do about F4 inversion? #else rxConfigMutable()->sbus_inversion = 1; // invert on F7 #endif -/* Breadboard-specific settings for development purposes only - */ + beeperOffSet((BEEPER_BAT_CRIT_LOW | BEEPER_BAT_LOW | BEEPER_RX_SET) ^ BEEPER_GYRO_CALIBRATED); + + /* Breadboard-specific settings for development purposes only + */ #if defined(BREADBOARD) boardAlignmentMutable()->pitchDegrees = 90; // vertical breakout board barometerConfigMutable()->baro_hardware = BARO_DEFAULT; // still testing not on V1 or V2 pcb +#else + barometerConfigMutable()->baro_hardware = BARO_NONE; #endif compassConfigMutable()->mag_hardware = MAG_DEFAULT; + + /* F4 (especially overclocked) and F7 ALIENWHOOP perform splendidly with 32kHz gyro enabled */ + gyroConfigMutable()->gyro_use_32khz = 1; + gyroConfigMutable()->gyro_sync_denom = 2; // 16kHz gyro + pidConfigMutable()->pid_process_denom = 1; // 16kHz PID + + featureSet((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); + + /* AlienWhoop PIDs based on Ole Gravy Leg (aka Matt Williamson's) PIDs + */ + for (int profileId = 0; profileId < MAX_PROFILE_COUNT; profileId++) + { + /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ + pidProfilesMutable(profileId)->pid[PID_PITCH].P = 75; + pidProfilesMutable(profileId)->pid[PID_PITCH].I = 36; + pidProfilesMutable(profileId)->pid[PID_PITCH].D = 25; + pidProfilesMutable(profileId)->pid[PID_ROLL].P = 75; + pidProfilesMutable(profileId)->pid[PID_ROLL].I = 36; + pidProfilesMutable(profileId)->pid[PID_ROLL].D = 25; + pidProfilesMutable(profileId)->pid[PID_YAW].P = 70; + pidProfilesMutable(profileId)->pid[PID_YAW].I = 36; + + pidProfilesMutable(profileId)->pid[PID_LEVEL].P = 30; + pidProfilesMutable(profileId)->pid[PID_LEVEL].D = 30; + + /* Setpoints */ + pidProfilesMutable(profileId)->dtermSetpointWeight = 100; + pidProfilesMutable(profileId)->setpointRelaxRatio = 100; // default to snappy for racers + + /* Throttle PID Attenuation (TPA) */ + pidProfilesMutable(profileId)->itermThrottleThreshold = 400; + } + + for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) + { + /* RC Rates */ + controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; + controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 100; + controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; + + /* Super Expo Rates */ + controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 80; + controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 80; + controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 85; + + /* Throttle PID Attenuation (TPA) */ + controlRateProfilesMutable(rateProfileId)->dynThrPID = 0; // tpa_rate off + controlRateProfilesMutable(rateProfileId)->tpa_breakpoint = 1600; + } } #endif From 877c82bc3ff2e87a2d7ebaa00d596d9d7741121c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 30 Aug 2017 09:50:23 +0100 Subject: [PATCH 014/138] Fix to angle mode limit --- src/main/fc/fc_msp.c | 4 ++-- src/main/fc/settings.c | 3 +-- src/main/flight/pid.c | 5 ++--- src/main/flight/pid.h | 1 - 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8281750ecc..d7e72068a5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1162,7 +1162,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, currentPidProfile->rateAccelLimit); sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit); sbufWriteU8(dst, currentPidProfile->levelAngleLimit); - sbufWriteU8(dst, currentPidProfile->levelSensitivity); + sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); break; @@ -1571,7 +1571,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentPidProfile->yawRateAccelLimit = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { currentPidProfile->levelAngleLimit = sbufReadU8(src); - currentPidProfile->levelSensitivity = sbufReadU8(src); + sbufReadU8(src); // was pidProfile.levelSensitivity } if (sbufBytesRemaining(src) >= 4) { currentPidProfile->itermThrottleThreshold = sbufReadU16(src); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 20b9132b67..4663ed9c60 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -594,8 +594,7 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) }, - { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelSensitivity) }, - { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 120 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, + { "level_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) }, { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, { "horizon_tilt_expert_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d723d0041d..b50ba0af6e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -69,7 +69,7 @@ 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, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2); void resetPidProfile(pidProfile_t *pidProfile) { @@ -98,7 +98,6 @@ void resetPidProfile(pidProfile_t *pidProfile) .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, - .levelSensitivity = 55, .setpointRelaxRatio = 100, .dtermSetpointWeight = 60, .yawRateAccelLimit = 100, @@ -354,7 +353,7 @@ static float calcHorizonLevelStrength(void) static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination // rcDeflection is in range [-1.0, 1.0] - float angle = pidProfile->levelSensitivity * getRcDeflection(axis); + float angle = pidProfile->levelAngleLimit * getRcDeflection(axis); #ifdef GPS angle += GPS_angle[axis]; #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e487e9e8cb..869e00cae1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -85,7 +85,6 @@ typedef struct pidProfile_s { uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. uint8_t levelAngleLimit; // Max angle in degrees in level mode - uint8_t levelSensitivity; // Angle mode sensitivity reflected in degrees assuming user using full stick uint8_t horizon_tilt_effect; // inclination factor for Horizon mode uint8_t horizon_tilt_expert_mode; // OFF or ON From c2b7be1039f02b16118194b5e34a3ec362f87071 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Wed, 30 Aug 2017 05:07:40 -0400 Subject: [PATCH 015/138] * Target ALIENWHOOP use K&R braces --- src/main/target/ALIENWHOOP/config.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index c52de76d88..cb9cba3fdb 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -65,8 +65,7 @@ void targetConfiguration(void) { - if (hardwareMotorType == MOTOR_BRUSHED) - { + if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; motorConfigMutable()->minthrottle = 1080; motorConfigMutable()->maxthrottle = 2000; @@ -105,8 +104,7 @@ void targetConfiguration(void) /* AlienWhoop PIDs based on Ole Gravy Leg (aka Matt Williamson's) PIDs */ - for (int profileId = 0; profileId < MAX_PROFILE_COUNT; profileId++) - { + for (int profileId = 0; profileId < MAX_PROFILE_COUNT; profileId++) { /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ pidProfilesMutable(profileId)->pid[PID_PITCH].P = 75; pidProfilesMutable(profileId)->pid[PID_PITCH].I = 36; @@ -128,8 +126,7 @@ void targetConfiguration(void) pidProfilesMutable(profileId)->itermThrottleThreshold = 400; } - for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) - { + for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { /* RC Rates */ controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 100; From 4715fde21d82dd3eb030577cfb554cb769d26be4 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Wed, 30 Aug 2017 11:45:06 +0300 Subject: [PATCH 016/138] F7 uart TX IT --- src/main/drivers/serial_uart_hal.c | 3 +- src/main/drivers/serial_uart_stm32f7xx.c | 90 +++++++++++++++--------- 2 files changed, 58 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 39e7ad350e..0b485da4e2 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -176,7 +176,8 @@ void uartReconfigure(uartPort_t *uartPort) __HAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0); } else { - __HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE); + /* Enable the UART Transmit Data Register Empty Interrupt */ + SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE); } } return; diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 8ab4aa7183..78c9ec8809 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -47,7 +47,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA2_Stream5, #endif +#ifdef USE_UART1_TX_DMA .txDMAStream = DMA2_Stream7, +#endif .rxPins = { DEFIO_TAG_E(PA10), DEFIO_TAG_E(PB7), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PA9), DEFIO_TAG_E(PB6), IO_TAG_NONE }, .af = GPIO_AF7_USART1, @@ -70,7 +72,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART2_RX_DMA .rxDMAStream = DMA1_Stream5, #endif +#ifdef USE_UART2_TX_DMA .txDMAStream = DMA1_Stream6, +#endif .rxPins = { DEFIO_TAG_E(PA3), DEFIO_TAG_E(PD6), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PA2), DEFIO_TAG_E(PD5), IO_TAG_NONE }, .af = GPIO_AF7_USART2, @@ -93,7 +97,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART3_RX_DMA .rxDMAStream = DMA1_Stream1, #endif +#ifdef USE_UART3_TX_DMA .txDMAStream = DMA1_Stream3, +#endif .rxPins = { DEFIO_TAG_E(PB11), DEFIO_TAG_E(PC11), DEFIO_TAG_E(PD9) }, .txPins = { DEFIO_TAG_E(PB10), DEFIO_TAG_E(PC10), DEFIO_TAG_E(PD8) }, .af = GPIO_AF7_USART3, @@ -116,7 +122,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART4_RX_DMA .rxDMAStream = DMA1_Stream2, #endif +#ifdef USE_UART4_TX_DMA .txDMAStream = DMA1_Stream4, +#endif .rxPins = { DEFIO_TAG_E(PA1), DEFIO_TAG_E(PC11), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PA0), DEFIO_TAG_E(PC10), IO_TAG_NONE }, .af = GPIO_AF8_UART4, @@ -139,7 +147,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART5_RX_DMA .rxDMAStream = DMA1_Stream0, #endif +#ifdef USE_UART5_TX_DMA .txDMAStream = DMA1_Stream7, +#endif .rxPins = { DEFIO_TAG_E(PD2), IO_TAG_NONE, IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PC12), IO_TAG_NONE, IO_TAG_NONE }, .af = GPIO_AF8_UART5, @@ -162,7 +172,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART6_RX_DMA .rxDMAStream = DMA2_Stream1, #endif +#ifdef USE_UART6_TX_DMA .txDMAStream = DMA2_Stream6, +#endif .rxPins = { DEFIO_TAG_E(PC7), DEFIO_TAG_E(PG9), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PC6), DEFIO_TAG_E(PG14), IO_TAG_NONE }, .af = GPIO_AF8_USART6, @@ -185,7 +197,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART7_RX_DMA .rxDMAStream = DMA1_Stream3, #endif +#ifdef USE_UART7_TX_DMA .txDMAStream = DMA1_Stream1, +#endif .rxPins = { DEFIO_TAG_E(PE7), DEFIO_TAG_E(PF6), IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PE8), DEFIO_TAG_E(PF7), IO_TAG_NONE }, .af = GPIO_AF8_UART7, @@ -208,7 +222,9 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART8_RX_DMA .rxDMAStream = DMA1_Stream6, #endif +#ifdef USE_UART8_TX_DMA .txDMAStream = DMA1_Stream0, +#endif .rxPins = { DEFIO_TAG_E(PE0), IO_TAG_NONE, IO_TAG_NONE }, .txPins = { DEFIO_TAG_E(PE1), IO_TAG_NONE, IO_TAG_NONE }, .af = GPIO_AF8_UART8, @@ -228,9 +244,8 @@ void uartIrqHandler(uartPort_t *s) { UART_HandleTypeDef *huart = &s->Handle; /* UART in mode Receiver ---------------------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) - { - uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff); + if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) { + uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t) 0xff); if (s->port.rxCallback) { s->port.rxCallback(rbyte); @@ -247,42 +262,51 @@ void uartIrqHandler(uartPort_t *s) } /* UART parity error interrupt occurred -------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF); + if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF); } /* UART frame error interrupt occurred --------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF); + if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF); } /* UART noise error interrupt occurred --------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF); + if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF); } /* UART Over-Run interrupt occurred -----------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) - { - __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF); + if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) { + __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF); } /* UART in mode Transmitter ------------------------------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) - { - HAL_UART_IRQHandler(huart); + if (!s->txDMAStream && (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) { + /* Check that a Tx process is ongoing */ + if (huart->gState != HAL_UART_STATE_BUSY_TX) { + if (s->port.txBufferTail == s->port.txBufferHead) { + huart->TxXferCount = 0; + /* Disable the UART Transmit Data Register Empty Interrupt */ + CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE); + } else { + if ((huart->Init.WordLength == UART_WORDLENGTH_9B) && (huart->Init.Parity == UART_PARITY_NONE)) { + huart->Instance->TDR = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU); + } else { + huart->Instance->TDR = (uint8_t)(s->port.txBuffer[s->port.txBufferTail]); + } + s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize; + } + } } /* UART in mode Transmitter (transmission end) -----------------------------*/ - if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) - { + if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) { HAL_UART_IRQHandler(huart); - handleUsartTxDma(s); + if (s->txDMAStream) { + handleUsartTxDma(s); + } } - } static void handleUsartTxDma(uartPort_t *s) @@ -329,8 +353,15 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, s->rxDMAChannel = hardware->DMAChannel; s->rxDMAStream = hardware->rxDMAStream; } - s->txDMAChannel = hardware->DMAChannel; - s->txDMAStream = hardware->txDMAStream; + + if (hardware->txDMAStream) { + s->txDMAChannel = hardware->DMAChannel; + s->txDMAStream = hardware->txDMAStream; + + // DMA TX Interrupt + dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); + dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev); + } s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; @@ -362,16 +393,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, } } - // DMA TX Interrupt - dmaInit(hardware->txIrq, OWNER_SERIAL_TX, RESOURCE_INDEX(device)); - dmaSetHandler(hardware->txIrq, dmaIRQHandler, hardware->txPriority, (uint32_t)uartdev); - - - //HAL_NVIC_SetPriority(hardware->txIrq, NVIC_PRIORITY_BASE(hardware->txPriority), NVIC_PRIORITY_SUB(hardware->txPriority)); - //HAL_NVIC_EnableIRQ(hardware->txIrq); - - if (!s->rxDMAChannel) - { + if (!s->rxDMAChannel) { HAL_NVIC_SetPriority(hardware->rxIrq, NVIC_PRIORITY_BASE(hardware->rxPriority), NVIC_PRIORITY_SUB(hardware->rxPriority)); HAL_NVIC_EnableIRQ(hardware->rxIrq); } From 652d9adba8add2aa33412965dc4169c38ba8caf7 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 31 Aug 2017 03:44:49 +0900 Subject: [PATCH 017/138] Avoid using N-Channel for RX function --- src/main/drivers/serial_softserial.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index aaf0b2a212..201a3742f4 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -254,8 +254,8 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb if (options & SERIAL_BIDIR) { // If RX and TX pins are both assigned, we CAN use either with a timer. // However, for consistency with hardware UARTs, we only use TX pin, - // and this pin must have a timer. - if (!timerTx) + // and this pin must have a timer, and it should not be N-Channel. + if (!(timerTx && !(timerTx->output & TIMER_OUTPUT_N_CHANNEL))) return NULL; softSerial->timerHardware = timerTx; @@ -264,8 +264,8 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET)); } else { if (mode & MODE_RX) { - // Need a pin & a timer on RX - if (!(tagRx && timerRx)) + // Need a pin & a timer on RX. Channel should not be N-Channel. + if (!(timerRx && !(timerRx->output & TIMER_OUTPUT_N_CHANNEL))) return NULL; softSerial->rxIO = rxIO; From ec4ce2bbd66366d805f7bab1a4291acb65fd811d Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 30 Aug 2017 21:28:49 +0200 Subject: [PATCH 018/138] Changed condition to USE_COPY_PROFILE_CMS_MENU --- src/main/cms/cms_menu_imu.c | 4 ++-- src/main/target/common_fc_pre.h | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 073e88617d..8107c1046b 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -391,7 +391,7 @@ static CMS_Menu cmsx_menuFilterPerProfile = { .entries = cmsx_menuFilterPerProfileEntries, }; -#ifndef DISABLE_EXTENDED_CMS_MENUS +#ifdef USE_COPY_PROFILE_CMS_MENU static uint8_t cmsx_dstPidProfile; static uint8_t cmsx_dstControlRateProfile; @@ -475,7 +475,7 @@ static OSD_Entry cmsx_menuImuEntries[] = {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, -#ifndef DISABLE_EXTENDED_CMS_MENUS +#ifdef USE_COPY_PROFILE_CMS_MENU {"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0}, #endif diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index d5104588f1..9f78bc5935 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -126,6 +126,7 @@ #define VTX_TRAMP #define USE_CAMERA_CONTROL #define USE_HUFFMAN +#define USE_COPY_PROFILE_CMS_MENU #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND From 19489af153bd16201c11fb748ea61ecd758720f6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 31 Aug 2017 04:02:15 +0900 Subject: [PATCH 019/138] Avoid N-Channel for RX side --- src/main/drivers/serial_escserial.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 31bd3b7205..25feddb389 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -660,6 +660,10 @@ static serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceive if (mode != PROTOCOL_KISSALL) { escSerial->rxTimerHardware = &(timerHardware[output]); + // N-Channels can't be used as RX. + if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + return NULL; + } #ifdef USE_HAL_DRIVER escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim); #endif From 4e68bc5895659b70fe616f44ebe8c1555f3553e4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 30 Aug 2017 23:45:37 +0100 Subject: [PATCH 020/138] Fixed gyro debug output --- src/main/sensors/gyro.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a2f89ae770..92871f3c79 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -364,6 +364,7 @@ bool gyroInit(void) case DEBUG_GYRO: case DEBUG_GYRO_RAW: gyroDebugMode = debugMode; + break; default: // debugMode is not gyro-related gyroDebugMode = DEBUG_NONE; From 5715bda90622e21c2076509f2c125c977e693416 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 31 Aug 2017 14:48:00 +0900 Subject: [PATCH 021/138] Tidy --- src/main/drivers/serial_softserial.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 201a3742f4..389596ec97 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -255,8 +255,9 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb // If RX and TX pins are both assigned, we CAN use either with a timer. // However, for consistency with hardware UARTs, we only use TX pin, // and this pin must have a timer, and it should not be N-Channel. - if (!(timerTx && !(timerTx->output & TIMER_OUTPUT_N_CHANNEL))) + if (!(timerTx && !(timerTx->output & TIMER_OUTPUT_N_CHANNEL))) { return NULL; + } softSerial->timerHardware = timerTx; softSerial->txIO = txIO; @@ -265,8 +266,9 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb } else { if (mode & MODE_RX) { // Need a pin & a timer on RX. Channel should not be N-Channel. - if (!(timerRx && !(timerRx->output & TIMER_OUTPUT_N_CHANNEL))) + if (!(timerRx && !(timerRx->output & TIMER_OUTPUT_N_CHANNEL))) { return NULL; + } softSerial->rxIO = rxIO; softSerial->timerHardware = timerRx; From b5ddae4672a61d4ea25296f47396bc7814baf81f Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 31 Aug 2017 15:02:21 +0900 Subject: [PATCH 022/138] For readability --- src/main/drivers/serial_softserial.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 389596ec97..6789d70ce1 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -255,7 +255,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb // If RX and TX pins are both assigned, we CAN use either with a timer. // However, for consistency with hardware UARTs, we only use TX pin, // and this pin must have a timer, and it should not be N-Channel. - if (!(timerTx && !(timerTx->output & TIMER_OUTPUT_N_CHANNEL))) { + if (!timerTx || (timerTx->output & TIMER_OUTPUT_N_CHANNEL)) { return NULL; } @@ -266,7 +266,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb } else { if (mode & MODE_RX) { // Need a pin & a timer on RX. Channel should not be N-Channel. - if (!(timerRx && !(timerRx->output & TIMER_OUTPUT_N_CHANNEL))) { + if (!timerRx || (timerRx->output & TIMER_OUTPUT_N_CHANNEL)) { return NULL; } From 6ab6ec377c103a9ef65a038576cb9ba74bb4878a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 31 Aug 2017 07:35:44 +0100 Subject: [PATCH 023/138] Code tidy and added/exposed minor utility functions --- src/main/common/streambuf.c | 12 ++++++++++++ src/main/common/streambuf.h | 2 ++ src/main/config/parameter_group.c | 2 +- src/main/config/parameter_group.h | 1 + src/main/fc/settings.h | 2 +- 5 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index c055ad61ae..976439ae81 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -20,6 +20,13 @@ #include "streambuf.h" +sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end) +{ + sbuf->ptr = ptr; + sbuf->end = end; + return sbuf; +} + void sbufWriteU8(sbuf_t *dst, uint8_t val) { *dst->ptr++ = val; @@ -65,6 +72,11 @@ void sbufWriteString(sbuf_t *dst, const char *string) sbufWriteData(dst, string, strlen(string)); } +void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string) +{ + sbufWriteData(dst, string, strlen(string) + 1); +} + uint8_t sbufReadU8(sbuf_t *src) { return *src->ptr++; diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index dad54adddb..790423350e 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -27,6 +27,7 @@ typedef struct sbuf_s { uint8_t *end; } sbuf_t; +sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end); void sbufWriteU8(sbuf_t *dst, uint8_t val); void sbufWriteU16(sbuf_t *dst, uint16_t val); void sbufWriteU32(sbuf_t *dst, uint32_t val); @@ -34,6 +35,7 @@ void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val); void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val); void sbufWriteData(sbuf_t *dst, const void *data, int len); void sbufWriteString(sbuf_t *dst, const char *string); +void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string); uint8_t sbufReadU8(sbuf_t *src); uint16_t sbufReadU16(sbuf_t *src); diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index a61b454820..30bf48430a 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -39,7 +39,7 @@ static uint8_t *pgOffset(const pgRegistry_t* reg) return reg->address; } -static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) +void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) { const uint16_t regSize = pgSize(reg); diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 16ab6c52fe..f64493ee87 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -187,5 +187,6 @@ const pgRegistry_t* pgFind(pgn_t pgn); void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version); int pgStore(const pgRegistry_t* reg, void *to, int size); void pgResetAll(); +void pgResetInstance(const pgRegistry_t *reg, uint8_t *base); bool pgResetCopy(void *copy, pgn_t pgn); void pgReset(const pgRegistry_t* reg); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 6dcf5d37f0..2b395105bc 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -119,7 +119,7 @@ typedef union { cliArrayLengthConfig_t array; } cliValueConfig_t; -typedef struct { +typedef struct clivalue_s { const char *name; const uint8_t type; // see cliValueFlag_e const cliValueConfig_t config; From e6bbce53e6e5f9a10d34e69e68928abe5e6c4f96 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 31 Aug 2017 13:09:20 +0100 Subject: [PATCH 024/138] Improved testability --- src/main/fc/cli.c | 20 +++++++++++++------- src/main/fc/cli.h | 4 ++++ src/main/fc/settings.h | 4 ++++ src/test/unit/cli_unittest.cc | 3 +-- src/test/unit/platform.h | 2 ++ 5 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 392e0dde7b..c695d8a901 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -402,12 +402,18 @@ static uint16_t getValueOffset(const clivalue_t *value) return 0; } -STATIC_UNIT_TESTED void *getValuePointer(const clivalue_t *value) +void *cliGetValuePointer(const clivalue_t *value) { const pgRegistry_t* rec = pgFind(value->pgn); return CONST_CAST(void *, rec->address + getValueOffset(value)); } +const void *cliGetDefaultPointer(const clivalue_t *value) +{ + const pgRegistry_t* rec = pgFind(value->pgn); + return rec->address + getValueOffset(value); +} + static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) { const pgRegistry_t *pg = pgFind(value->pgn); @@ -448,7 +454,7 @@ static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) static void cliPrintVar(const clivalue_t *var, bool full) { - const void *ptr = getValuePointer(var); + const void *ptr = cliGetValuePointer(var); printValuePointer(var, ptr, full); } @@ -481,7 +487,7 @@ static void cliPrintVarRange(const clivalue_t *var) static void cliSetVar(const clivalue_t *var, const int16_t value) { - void *ptr = getValuePointer(var); + void *ptr = cliGetValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -2788,7 +2794,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) default: case VAR_UINT8: { // fetch data pointer - uint8_t *data = (uint8_t *)getValuePointer(val) + i; + uint8_t *data = (uint8_t *)cliGetValuePointer(val) + i; // store value *data = (uint8_t)atoi((const char*) valPtr); } @@ -2796,7 +2802,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) case VAR_INT8: { // fetch data pointer - int8_t *data = (int8_t *)getValuePointer(val) + i; + int8_t *data = (int8_t *)cliGetValuePointer(val) + i; // store value *data = (int8_t)atoi((const char*) valPtr); } @@ -2804,7 +2810,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) case VAR_UINT16: { // fetch data pointer - uint16_t *data = (uint16_t *)getValuePointer(val) + i; + uint16_t *data = (uint16_t *)cliGetValuePointer(val) + i; // store value *data = (uint16_t)atoi((const char*) valPtr); } @@ -2812,7 +2818,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) case VAR_INT16: { // fetch data pointer - int16_t *data = (int16_t *)getValuePointer(val) + i; + int16_t *data = (int16_t *)cliGetValuePointer(val) + i; // store value *data = (int16_t)atoi((const char*) valPtr); } diff --git a/src/main/fc/cli.h b/src/main/fc/cli.h index a0ad8bee53..d1624476c3 100644 --- a/src/main/fc/cli.h +++ b/src/main/fc/cli.h @@ -19,6 +19,10 @@ extern uint8_t cliMode; +struct clivalue_s; +void *cliGetValuePointer(const struct clivalue_s *value); +const void *cliGetDefaultPointer(const struct clivalue_s *value); + struct serialConfig_s; void cliInit(const struct serialConfig_s *serialConfig); void cliProcess(void); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 2b395105bc..d82fc440b2 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -17,6 +17,10 @@ #pragma once +#include +#include +#include "config/parameter_group.h" + typedef enum { TABLE_OFF_ON = 0, diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 68a1bbb5c6..34f70c81fd 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -53,7 +53,6 @@ extern "C" { void cliSet(char *cmdline); void cliGet(char *cmdline); - void *getValuePointer(const clivalue_t *value); const clivalue_t valueTable[] = { { "array_unit_test", VAR_INT8 | MODE_ARRAY | MASTER_VALUE, .config.array.length = 3, PG_RESERVED_FOR_TESTING_1, 0 } @@ -99,7 +98,7 @@ TEST(CLIUnittest, TestCliSet) }; printf("\n===============================\n"); - int8_t *data = (int8_t *)getValuePointer(&cval); + int8_t *data = (int8_t *)cliGetValuePointer(&cval); for(int i=0; i<3; i++){ printf("data[%d] = %d\n", i, data[i]); } diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 262fe8d98f..f48b34b5ce 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,8 @@ #pragma once +#include + #define USE_PARAMETER_GROUPS #define U_ID_0 0 From 407566cbd12a081a1a1790ce0b12e554ee512e2a Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 31 Aug 2017 22:17:16 +0900 Subject: [PATCH 025/138] Added comment about eventual removal of IOConfigGPIOAF from timerInit. --- src/main/drivers/timer.c | 1 + src/main/drivers/timer_hal.c | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index d51aa743eb..70e4b9c202 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -712,6 +712,7 @@ void timerInit(void) if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { continue; } + // XXX IOConfigGPIOAF in timerInit should eventually go away. IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 4aeb43ca44..ddab0ad285 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -810,6 +810,7 @@ void timerInit(void) if (timerHardwarePtr->usageFlags == TIM_USE_NONE) { continue; } + // XXX IOConfigGPIOAF in timerInit should eventually go away. IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), IOCFG_AF_PP, timerHardwarePtr->alternateFunction); } #endif From 604b661a62f49d174cf4f37f13d123a227594a94 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 31 Aug 2017 17:02:49 +0100 Subject: [PATCH 026/138] Rationalised CRSF constants --- src/main/rx/crsf.h | 2 +- src/main/telemetry/crsf.c | 34 +++++++++++++++--------- src/main/telemetry/crsf.h | 9 +------ src/test/unit/telemetry_crsf_unittest.cc | 20 +++++++------- 4 files changed, 33 insertions(+), 32 deletions(-) diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 7b93bf8df8..383ac0ac74 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -30,7 +30,7 @@ typedef enum { CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, CRSF_FRAMETYPE_FLIGHT_MODE = 0x21 -} crsfFrameTypes_e; +} crsfFrameType_e; enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 6231b5e6d1..6d041ebcd6 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -226,7 +226,15 @@ void crsfFrameFlightMode(sbuf_t *dst) #define BV(x) (1 << (x)) // bit value // schedule array to decide how often each type of frame is sent -#define CRSF_SCHEDULE_COUNT_MAX 5 +typedef enum { + CRSF_FRAME_START_INDEX = 0, + CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX, + CRSF_FRAME_BATTERY_SENSOR_INDEX, + CRSF_FRAME_FLIGHT_MODE_INDEX, + CRSF_FRAME_GPS_INDEX, + CRSF_SCHEDULE_COUNT_MAX +} crsfFrameTypeIndex_e; + static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; @@ -239,23 +247,23 @@ static void processCrsf(void) sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; - if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) { + if (currentSchedule & BV(CRSF_FRAME_ATTITUDE_INDEX)) { crsfInitializeFrame(dst); crsfFrameAttitude(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) { + if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR_INDEX)) { crsfInitializeFrame(dst); crsfFrameBatterySensor(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { + if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) { crsfInitializeFrame(dst); crsfFrameFlightMode(dst); crsfFinalize(dst); } #ifdef GPS - if (currentSchedule & BV(CRSF_FRAME_GPS)) { + if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { crsfInitializeFrame(dst); crsfFrameGps(dst); crsfFinalize(dst); @@ -270,11 +278,11 @@ void initCrsfTelemetry(void) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); int index = 0; - crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE); - crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR); - crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE); + crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); if (feature(FEATURE_GPS)) { - crsfSchedule[index++] = BV(CRSF_FRAME_GPS); + crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } crsfScheduleCount = (uint8_t)index; @@ -315,17 +323,17 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) crsfInitializeFrame(sbuf); switch (frameType) { default: - case CRSF_FRAME_ATTITUDE: + case CRSF_FRAMETYPE_ATTITUDE: crsfFrameAttitude(sbuf); break; - case CRSF_FRAME_BATTERY_SENSOR: + case CRSF_FRAMETYPE_BATTERY_SENSOR: crsfFrameBatterySensor(sbuf); break; - case CRSF_FRAME_FLIGHT_MODE: + case CRSF_FRAMETYPE_FLIGHT_MODE: crsfFrameFlightMode(sbuf); break; #if defined(GPS) - case CRSF_FRAME_GPS: + case CRSF_FRAMETYPE_GPS: crsfFrameGps(sbuf); break; #endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 980467f517..e5a3e4611e 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -18,14 +18,7 @@ #pragma once #include "common/time.h" - -typedef enum { - CRSF_FRAME_START = 0, - CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, - CRSF_FRAME_BATTERY_SENSOR, - CRSF_FRAME_FLIGHT_MODE, - CRSF_FRAME_GPS -} crsfFrameType_e; +#include "rx/crsf.h" void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 947be24296..a14e00bc0b 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -91,7 +91,7 @@ TEST(TelemetryCrsfTest, TestGPS) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - int frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS); EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(17, frame[1]); // length @@ -117,7 +117,7 @@ TEST(TelemetryCrsfTest, TestGPS) gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 gpsSol.numSat = 9; gpsSol.groundCourse = 1479; // degrees * 10 - frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS); lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; EXPECT_EQ(560000000, lattitude); longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10]; @@ -138,7 +138,7 @@ TEST(TelemetryCrsfTest, TestBattery) uint8_t frame[CRSF_FRAME_SIZE_MAX]; testBatteryVoltage = 0; // 0.1V units - int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(10, frame[1]); // length @@ -156,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery) testBatteryVoltage = 33; // 3.3V = 3300 mv testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps batteryConfigMutable()->batteryCapacity = 1234; - frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); current = frame[5] << 8 | frame[6]; // mA * 100 @@ -175,7 +175,7 @@ TEST(TelemetryCrsfTest, TestAttitude) attitude.values.pitch = 0; attitude.values.roll = 0; attitude.values.yaw = 0; - int frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE); EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(8, frame[1]); // length @@ -191,7 +191,7 @@ TEST(TelemetryCrsfTest, TestAttitude) attitude.values.pitch = 678; // decidegrees == 1.183333232852155 rad attitude.values.roll = 1495; // 2.609267231731523 rad attitude.values.yaw = -1799; //3.139847324337799 rad - frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE); pitch = frame[3] << 8 | frame[4]; // rad / 10000 EXPECT_EQ(11833, pitch); roll = frame[5] << 8 | frame[6]; @@ -207,7 +207,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) // nothing set, so ACRO mode airMode = false; - int frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(7, frame[1]); // length @@ -222,7 +222,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) enableFlightMode(ANGLE_MODE); EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE)); - frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(7, frame[1]); // length @@ -237,7 +237,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) disableFlightMode(ANGLE_MODE); enableFlightMode(HORIZON_MODE); EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE)); - frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(6, frame[1]); // length @@ -250,7 +250,7 @@ TEST(TelemetryCrsfTest, TestFlightMode) disableFlightMode(HORIZON_MODE); airMode = true; - frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address EXPECT_EQ(6, frame[1]); // length From 6d073dfe64697568ae5b38cec2b1800a562db03c Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Wed, 30 Aug 2017 05:46:46 -0400 Subject: [PATCH 027/138] * Target NAZE fix profile default settings loops --- src/main/target/NAZE/config.c | 42 +++++++++++++++++------------------ 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index dc198f4c6a..e7e4c63fcb 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -61,29 +61,29 @@ void targetConfiguration(void) rxChannelRangeConfigsMutable(channel)->max = 1860; }*/ - for (int profileId = 0; profileId < 2; profileId++) { - pidProfilesMutable(0)->pid[PID_ROLL].P = 60; - pidProfilesMutable(0)->pid[PID_ROLL].I = 70; - pidProfilesMutable(0)->pid[PID_ROLL].D = 17; - pidProfilesMutable(0)->pid[PID_PITCH].P = 80; - pidProfilesMutable(0)->pid[PID_PITCH].I = 90; - pidProfilesMutable(0)->pid[PID_PITCH].D = 18; - pidProfilesMutable(0)->pid[PID_YAW].P = 200; - pidProfilesMutable(0)->pid[PID_YAW].I = 45; - pidProfilesMutable(0)->pid[PID_LEVEL].P = 30; - pidProfilesMutable(0)->pid[PID_LEVEL].D = 30; + for (int profileId = 0; profileId < MAX_PROFILE_COUNT; profileId++) { + pidProfilesMutable(profileId)->pid[PID_ROLL].P = 60; + pidProfilesMutable(profileId)->pid[PID_ROLL].I = 70; + pidProfilesMutable(profileId)->pid[PID_ROLL].D = 17; + pidProfilesMutable(profileId)->pid[PID_PITCH].P = 80; + pidProfilesMutable(profileId)->pid[PID_PITCH].I = 90; + pidProfilesMutable(profileId)->pid[PID_PITCH].D = 18; + pidProfilesMutable(profileId)->pid[PID_YAW].P = 200; + pidProfilesMutable(profileId)->pid[PID_YAW].I = 45; + pidProfilesMutable(profileId)->pid[PID_LEVEL].P = 30; + pidProfilesMutable(profileId)->pid[PID_LEVEL].D = 30; - for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { - controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; - controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 110; - controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; - controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 77; - controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 77; - controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 80; + pidProfilesMutable(profileId)->dtermSetpointWeight = 200; + pidProfilesMutable(profileId)->setpointRelaxRatio = 50; + } - pidProfilesMutable(0)->dtermSetpointWeight = 200; - pidProfilesMutable(0)->setpointRelaxRatio = 50; - } + for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { + controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; + controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 110; + controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; + controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 77; + controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 77; + controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 80; } #endif From f2a937ef5bb4f6ddb45200a5ec04d0628064d944 Mon Sep 17 00:00:00 2001 From: Sean Blakemore Date: Fri, 18 Aug 2017 15:08:32 +1000 Subject: [PATCH 028/138] Enable OSD support for future hardware --- src/main/target/IMPULSERCF3/target.h | 8 ++++++++ src/main/target/IMPULSERCF3/target.mk | 3 ++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 3e2213b3cf..ba721a47d4 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -79,6 +79,14 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PA3 +#define MAX7456_NRST_PIN PC14 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/IMPULSERCF3/target.mk b/src/main/target/IMPULSERCF3/target.mk index 53b083d225..db716cfd30 100644 --- a/src/main/target/IMPULSERCF3/target.mk +++ b/src/main/target/IMPULSERCF3/target.mk @@ -4,4 +4,5 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/flash_m25p16.c + drivers/flash_m25p16.c \ + drivers/max7456.c From e59ab3c7254dd972d9f3a4867be6cfcfdb21ddd8 Mon Sep 17 00:00:00 2001 From: Sean Blakemore Date: Fri, 18 Aug 2017 15:16:57 +1000 Subject: [PATCH 029/138] Add some defaults to target Enable current sensor and set scale Default to SBUS RX --- src/main/target/IMPULSERCF3/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index ba721a47d4..e198c367fa 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/src/main/target/IMPULSERCF3/target.h @@ -88,15 +88,19 @@ #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define USE_ADC #define ADC_INSTANCE ADC1 #define CURRENT_METER_ADC_PIN PA0 #define RSSI_ADC_PIN PA1 #define VBAT_ADC_PIN PA2 +#define CURRENT_METER_SCALE_DEFAULT 275 + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 #define USE_SERIAL_4WAY_BLHELI_INTERFACE From 3f037da16f693917dbc6abd4c28a4dbd068bc97d Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Fri, 1 Sep 2017 01:17:01 -0400 Subject: [PATCH 030/138] * Code cleanup per @diehertz request (now storing result of pidProfilesMutable() and controlRateProfilesMutable() within for loops) --- src/main/target/NAZE/config.c | 44 +++++++++++++++++++---------------- 1 file changed, 24 insertions(+), 20 deletions(-) diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index e7e4c63fcb..1a1f894a87 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -61,29 +61,33 @@ void targetConfiguration(void) rxChannelRangeConfigsMutable(channel)->max = 1860; }*/ - for (int profileId = 0; profileId < MAX_PROFILE_COUNT; profileId++) { - pidProfilesMutable(profileId)->pid[PID_ROLL].P = 60; - pidProfilesMutable(profileId)->pid[PID_ROLL].I = 70; - pidProfilesMutable(profileId)->pid[PID_ROLL].D = 17; - pidProfilesMutable(profileId)->pid[PID_PITCH].P = 80; - pidProfilesMutable(profileId)->pid[PID_PITCH].I = 90; - pidProfilesMutable(profileId)->pid[PID_PITCH].D = 18; - pidProfilesMutable(profileId)->pid[PID_YAW].P = 200; - pidProfilesMutable(profileId)->pid[PID_YAW].I = 45; - pidProfilesMutable(profileId)->pid[PID_LEVEL].P = 30; - pidProfilesMutable(profileId)->pid[PID_LEVEL].D = 30; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - pidProfilesMutable(profileId)->dtermSetpointWeight = 200; - pidProfilesMutable(profileId)->setpointRelaxRatio = 50; + pidProfile->pid[PID_ROLL].P = 60; + pidProfile->pid[PID_ROLL].I = 70; + pidProfile->pid[PID_ROLL].D = 17; + pidProfile->pid[PID_PITCH].P = 80; + pidProfile->pid[PID_PITCH].I = 90; + pidProfile->pid[PID_PITCH].D = 18; + pidProfile->pid[PID_YAW].P = 200; + pidProfile->pid[PID_YAW].I = 45; + pidProfile->pid[PID_LEVEL].P = 30; + pidProfile->pid[PID_LEVEL].D = 30; + + pidProfile->dtermSetpointWeight = 200; + pidProfile->setpointRelaxRatio = 50; } - for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { - controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; - controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 110; - controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; - controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 77; - controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 77; - controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 80; + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 110; + controlRateConfig->rcExpo8 = 0; + controlRateConfig->rates[FD_ROLL] = 77; + controlRateConfig->rates[FD_PITCH] = 77; + controlRateConfig->rates[FD_YAW] = 80; } #endif From 8030a57d908b19f8e93e96bfff8c2c6a6cf536b2 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Fri, 1 Sep 2017 02:11:33 -0400 Subject: [PATCH 031/138] Use variable to access pid and rate profile defaults in loop --- src/main/target/ALIENWHOOP/config.c | 52 ++++++++++++++++------------- 1 file changed, 28 insertions(+), 24 deletions(-) diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index cb9cba3fdb..b26d223626 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -104,42 +104,46 @@ void targetConfiguration(void) /* AlienWhoop PIDs based on Ole Gravy Leg (aka Matt Williamson's) PIDs */ - for (int profileId = 0; profileId < MAX_PROFILE_COUNT; profileId++) { - /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ - pidProfilesMutable(profileId)->pid[PID_PITCH].P = 75; - pidProfilesMutable(profileId)->pid[PID_PITCH].I = 36; - pidProfilesMutable(profileId)->pid[PID_PITCH].D = 25; - pidProfilesMutable(profileId)->pid[PID_ROLL].P = 75; - pidProfilesMutable(profileId)->pid[PID_ROLL].I = 36; - pidProfilesMutable(profileId)->pid[PID_ROLL].D = 25; - pidProfilesMutable(profileId)->pid[PID_YAW].P = 70; - pidProfilesMutable(profileId)->pid[PID_YAW].I = 36; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - pidProfilesMutable(profileId)->pid[PID_LEVEL].P = 30; - pidProfilesMutable(profileId)->pid[PID_LEVEL].D = 30; + /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ + pidProfile->pid[PID_PITCH].P = 75; + pidProfile->pid[PID_PITCH].I = 36; + pidProfile->pid[PID_PITCH].D = 25; + pidProfile->pid[PID_ROLL].P = 75; + pidProfile->pid[PID_ROLL].I = 36; + pidProfile->pid[PID_ROLL].D = 25; + pidProfile->pid[PID_YAW].P = 70; + pidProfile->pid[PID_YAW].I = 36; + + pidProfile->pid[PID_LEVEL].P = 30; + pidProfile->pid[PID_LEVEL].D = 30; /* Setpoints */ - pidProfilesMutable(profileId)->dtermSetpointWeight = 100; - pidProfilesMutable(profileId)->setpointRelaxRatio = 100; // default to snappy for racers + pidProfile->dtermSetpointWeight = 100; + pidProfile->setpointRelaxRatio = 100; // default to snappy for racers /* Throttle PID Attenuation (TPA) */ - pidProfilesMutable(profileId)->itermThrottleThreshold = 400; + pidProfile->itermThrottleThreshold = 400; } - for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) { + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + /* RC Rates */ - controlRateProfilesMutable(rateProfileId)->rcRate8 = 100; - controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 100; - controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0; + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 100; + controlRateConfig->rcExpo8 = 0; /* Super Expo Rates */ - controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 80; - controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 80; - controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 85; + controlRateConfig->rates[FD_ROLL] = 80; + controlRateConfig->rates[FD_PITCH] = 80; + controlRateConfig->rates[FD_YAW] = 85; /* Throttle PID Attenuation (TPA) */ - controlRateProfilesMutable(rateProfileId)->dynThrPID = 0; // tpa_rate off - controlRateProfilesMutable(rateProfileId)->tpa_breakpoint = 1600; + controlRateConfig->dynThrPID = 0; // tpa_rate off + controlRateConfig->tpa_breakpoint = 1600; } } #endif From 7740a65e8a346a65c964c421712bdb897bca7209 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 1 Sep 2017 07:21:11 +0100 Subject: [PATCH 032/138] Fixed whitespace in CLI switch statements --- src/main/fc/cli.c | 178 +++++++++++++++++++++++----------------------- 1 file changed, 89 insertions(+), 89 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c695d8a901..bd1847dc3d 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -894,18 +894,18 @@ static void cliSerialPassthrough(char *cmdline) while (tok != NULL) { switch (index) { - case 0: - id = atoi(tok); - break; - case 1: - baud = atoi(tok); - break; - case 2: - if (strstr(tok, "rx") || strstr(tok, "RX")) - mode |= MODE_RX; - if (strstr(tok, "tx") || strstr(tok, "TX")) - mode |= MODE_TX; - break; + case 0: + id = atoi(tok); + break; + case 1: + baud = atoi(tok); + break; + case 2: + if (strstr(tok, "rx") || strstr(tok, "RX")) + mode |= MODE_RX; + if (strstr(tok, "tx") || strstr(tok, "TX")) + mode |= MODE_TX; + break; } index++; tok = strtok_r(NULL, " ", &saveptr); @@ -1702,28 +1702,28 @@ static void cliSdInfo(char *cmdline) cliPrint("'\r\n" "Filesystem: "); switch (afatfs_getFilesystemState()) { - case AFATFS_FILESYSTEM_STATE_READY: - cliPrint("Ready"); + case AFATFS_FILESYSTEM_STATE_READY: + cliPrint("Ready"); break; - case AFATFS_FILESYSTEM_STATE_INITIALIZATION: - cliPrint("Initializing"); + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + cliPrint("Initializing"); break; - case AFATFS_FILESYSTEM_STATE_UNKNOWN: - case AFATFS_FILESYSTEM_STATE_FATAL: - cliPrint("Fatal"); + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + case AFATFS_FILESYSTEM_STATE_FATAL: + cliPrint("Fatal"); - switch (afatfs_getLastError()) { - case AFATFS_ERROR_BAD_MBR: - cliPrint(" - no FAT MBR partitions"); - break; - case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: - cliPrint(" - bad FAT header"); - break; - case AFATFS_ERROR_GENERIC: - case AFATFS_ERROR_NONE: - ; // Nothing more detailed to print - break; - } + switch (afatfs_getLastError()) { + case AFATFS_ERROR_BAD_MBR: + cliPrint(" - no FAT MBR partitions"); + break; + case AFATFS_ERROR_BAD_FILESYSTEM_HEADER: + cliPrint(" - bad FAT header"); + break; + case AFATFS_ERROR_GENERIC: + case AFATFS_ERROR_NONE: + ; // Nothing more detailed to print + break; + } break; } cliPrintLinefeed(); @@ -2373,44 +2373,44 @@ static void cliDshotProg(char *cmdline) int escIndex = 0; while (pch != NULL) { switch (pos) { - case 0: - escIndex = parseOutputIndex(pch, true); - if (escIndex == -1) { - return; + case 0: + escIndex = parseOutputIndex(pch, true); + if (escIndex == -1) { + return; + } + + break; + default: + pwmDisableMotors(); + + int command = atoi(pch); + if (command >= 0 && command < DSHOT_MIN_THROTTLE) { + if (command == DSHOT_CMD_ESC_INFO) { + delay(5); // Wait for potential ESC telemetry transmission to finish } - break; - default: - pwmDisableMotors(); - - int command = atoi(pch); - if (command >= 0 && command < DSHOT_MIN_THROTTLE) { - if (command == DSHOT_CMD_ESC_INFO) { - delay(5); // Wait for potential ESC telemetry transmission to finish - } - - if (command != DSHOT_CMD_ESC_INFO) { - pwmWriteDshotCommand(escIndex, getMotorCount(), command); + if (command != DSHOT_CMD_ESC_INFO) { + pwmWriteDshotCommand(escIndex, getMotorCount(), command); + } else { + if (escIndex != ALL_MOTORS) { + executeEscInfoCommand(escIndex); } else { - if (escIndex != ALL_MOTORS) { - executeEscInfoCommand(escIndex); - } else { - for (uint8_t i = 0; i < getMotorCount(); i++) { - executeEscInfoCommand(i); - } + for (uint8_t i = 0; i < getMotorCount(); i++) { + executeEscInfoCommand(i); } } - - cliPrintLinef("Command %d written.", command); - - if (command <= 5) { - delay(20); // wait for sound output to finish - } - } else { - cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1); } - break; + cliPrintLinef("Command %d written.", command); + + if (command <= 5) { + delay(20); // wait for sound output to finish + } + } else { + cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1); + } + + break; } pos++; @@ -2437,34 +2437,34 @@ static void cliEscPassthrough(char *cmdline) int escIndex = 0; while (pch != NULL) { switch (pos) { - case 0: - if (strncasecmp(pch, "sk", strlen(pch)) == 0) { - mode = PROTOCOL_SIMONK; - } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) { - mode = PROTOCOL_BLHELI; - } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) { - mode = PROTOCOL_KISS; - } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) { - mode = PROTOCOL_KISSALL; - } else { - cliShowParseError(); - - return; - } - break; - case 1: - escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS); - if (escIndex == -1) { - return; - } - - break; - default: + case 0: + if (strncasecmp(pch, "sk", strlen(pch)) == 0) { + mode = PROTOCOL_SIMONK; + } else if (strncasecmp(pch, "bl", strlen(pch)) == 0) { + mode = PROTOCOL_BLHELI; + } else if (strncasecmp(pch, "ki", strlen(pch)) == 0) { + mode = PROTOCOL_KISS; + } else if (strncasecmp(pch, "cc", strlen(pch)) == 0) { + mode = PROTOCOL_KISSALL; + } else { cliShowParseError(); return; + } + break; + case 1: + escIndex = parseOutputIndex(pch, mode == PROTOCOL_KISS); + if (escIndex == -1) { + return; + } - break; + break; + default: + cliShowParseError(); + + return; + + break; } pos++; @@ -2749,7 +2749,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) bool valueChanged = false; int16_t value = 0; switch (val->type & VALUE_MODE_MASK) { - case MODE_DIRECT: { + case MODE_DIRECT: { int16_t value = atoi(eqptr); if (value >= val->config.minmax.min && value <= val->config.minmax.max) { @@ -2759,7 +2759,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) } break; - case MODE_LOOKUP: { + case MODE_LOOKUP: { const lookupTableEntry_t *tableEntry = &lookupTables[val->config.lookup.tableIndex]; bool matched = false; for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { @@ -2775,7 +2775,7 @@ STATIC_UNIT_TESTED void cliSet(char *cmdline) } break; - case MODE_ARRAY: { + case MODE_ARRAY: { const uint8_t arrayLength = val->config.array.length; char *valPtr = eqptr; From e1e338c15da65db754a6274f81847521c71e7251 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 1 Sep 2017 18:14:45 +0900 Subject: [PATCH 033/138] Explicitly call IOConfigGPIOAF for F3 and F4. --- src/main/drivers/serial_softserial.c | 26 +++++++++++--------------- src/main/target/OMNIBUS/target.h | 3 ++- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 6789d70ce1..d1396e89dd 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -131,18 +131,14 @@ static void serialInputPortActivate(softSerial_t *softSerial) if (softSerial->port.options & SERIAL_INVERTED) { #ifdef STM32F1 IOConfigGPIO(softSerial->rxIO, IOCFG_IPD); -#elif defined(STM32F7) - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction); #else - IOConfigGPIO(softSerial->rxIO, IOCFG_AF_PP_PD); + IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction); #endif } else { #ifdef STM32F1 IOConfigGPIO(softSerial->rxIO, IOCFG_IPU); -#elif defined(STM32F7) - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction); #else - IOConfigGPIO(softSerial->rxIO, IOCFG_AF_PP_UP); + IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction); #endif } @@ -165,35 +161,35 @@ static void serialInputPortDeActivate(softSerial_t *softSerial) TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable); #endif -#ifdef STM32F7 - IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction); -#else +#ifdef STM32F1 IOConfigGPIO(softSerial->rxIO, IOCFG_IN_FLOATING); +#else + IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction); #endif softSerial->rxActive = false; } static void serialOutputPortActivate(softSerial_t *softSerial) { -#ifdef STM32F7 +#ifdef STM32F1 + IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); +#else if (softSerial->exTimerHardware) IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTimerHardware->alternateFunction); else IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); -#else - IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP); #endif } static void serialOutputPortDeActivate(softSerial_t *softSerial) { -#ifdef STM32F7 +#ifdef STM32F1 + IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); +#else if (softSerial->exTimerHardware) IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTimerHardware->alternateFunction); else IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); -#else - IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING); #endif } diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 2e5353a7a6..183df84896 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -19,7 +19,8 @@ #undef TELEMETRY_IBUS //no space left #undef TELEMETRY_HOTT //no space left -#undef TELEMETRY_JETIEXBUS +#undef TELEMETRY_JETIEXBUS // no space left +#undef TELEMETRY_MAVLINK // no space left #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus From afec0258c778f8e64a03910df77a0f19b349cf77 Mon Sep 17 00:00:00 2001 From: Brian Balogh Date: Fri, 1 Sep 2017 10:57:54 -0400 Subject: [PATCH 034/138] Add support for ICM-20649 acc/gyro --- src/main/drivers/accgyro/accgyro_mpu.c | 17 ++ src/main/drivers/accgyro/accgyro_mpu.h | 5 +- .../drivers/accgyro/accgyro_spi_icm20649.c | 203 ++++++++++++++++++ .../drivers/accgyro/accgyro_spi_icm20649.h | 64 ++++++ src/main/fc/fc_init.c | 3 + src/main/fc/settings.c | 6 +- src/main/sensors/acceleration.c | 12 ++ src/main/sensors/acceleration.h | 1 + src/main/sensors/gyro.c | 18 +- src/main/sensors/gyro.h | 1 + 10 files changed, 325 insertions(+), 5 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_spi_icm20649.c create mode 100644 src/main/drivers/accgyro/accgyro_spi_icm20649.h diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index c7a10665bf..ed7e59451f 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -289,6 +290,22 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) } #endif +#ifdef USE_GYRO_SPI_ICM20649 +#ifdef ICM20649_SPI_INSTANCE + spiBusSetInstance(&gyro->bus, ICM20649_SPI_INSTANCE); +#endif +#ifdef ICM20649_CS_PIN + gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20649_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; +#endif + sensor = icm20649SpiDetect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiBusWriteRegister; + return true; + } +#endif + #ifdef USE_GYRO_SPI_ICM20689 #ifdef ICM20689_SPI_INSTANCE spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 04d27c084e..ed2f7ca977 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -23,7 +23,8 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT -#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) #define GYRO_USES_SPI #endif @@ -40,6 +41,7 @@ #define ICM20601_WHO_AM_I_CONST (0xAC) #define ICM20602_WHO_AM_I_CONST (0x12) #define ICM20608G_WHO_AM_I_CONST (0xAF) +#define ICM20649_WHO_AM_I_CONST (0xE1) #define ICM20689_WHO_AM_I_CONST (0x98) @@ -189,6 +191,7 @@ typedef enum { ICM_20601_SPI, ICM_20602_SPI, ICM_20608_SPI, + ICM_20649_SPI, ICM_20689_SPI, BMI_160_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c new file mode 100644 index 0000000000..fb0030951d --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -0,0 +1,203 @@ +/* + * 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 "common/maths.h" + +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/gyro_sync.h" +#include "drivers/io.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +#include "accgyro.h" +#include "accgyro_mpu.h" +#include "accgyro_spi_icm20649.h" + +static bool use4kDps = true; // TODO: make these configurable for testing +static bool use30g = true; + +static void icm20649SpiInit(const busDevice_t *bus) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + + IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); + + // all registers can be read/written at full speed (7MHz +-10%) + // TODO verify that this works at 9MHz and 10MHz on non F7 + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + hardwareInitialised = true; +} + +uint8_t icm20649SpiDetect(const busDevice_t *bus) +{ + icm20649SpiInit(bus); + + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + spiBusWriteRegister(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + delay(15); + + spiBusWriteRegister(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + + uint8_t icmDetected = MPU_NONE; + uint8_t attemptsRemaining = 20; + do { + delay(150); + const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I); + if (whoAmI == ICM20649_WHO_AM_I_CONST) { + icmDetected = ICM_20649_SPI; + } else { + icmDetected = MPU_NONE; + } + if (icmDetected != MPU_NONE) { + break; + } + if (!attemptsRemaining) { + return MPU_NONE; + } + } while (attemptsRemaining--); + + return icmDetected; + +} + +void icm20649AccInit(accDev_t *acc) +{ + // 2,048 LSB/g 16g + // 1,024 LSB/g 30g + acc->acc_1G = use30g ? 1024 : 2048; + + spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + delay(15); + const uint8_t acc_fsr = use30g ? ICM20649_FSR_30G : ICM20649_FSR_16G; + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); + delay(15); + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 + delay(15); +} + +bool icm20649SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != ICM_20649_SPI) { + return false; + } + + acc->initFn = icm20649AccInit; + acc->readFn = icm20649AccRead; + + return true; +} + + +void icm20649GyroInit(gyroDev_t *gyro) +{ + mpuGyroInit(gyro); + + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed + + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + delay(100); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + delay(15); + const uint8_t gyro_fsr = use4kDps ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; + uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1_kHz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) + raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3; + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + delay(100); + + // Data ready interrupt configuration + // back to bank 0 + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN + delay(15); + +#ifdef USE_MPU_DATA_READY_SIGNAL + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); +#endif +} + +bool icm20649SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != ICM_20649_SPI) + return false; + + gyro->initFn = icm20649GyroInit; + gyro->readFn = icm20649GyroReadSPI; + + // 16.4 dps/lsb 2kDps + // 8.2 dps/lsb 4kDps + gyro->scale = 1.0f / (use4kDps ? 8.2f : 16.4f); + + return true; +} + +bool icm20649GyroReadSPI(gyroDev_t *gyro) +{ + static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + uint8_t data[7]; + + const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); + gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); + gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + + return true; +} + +bool icm20649AccRead(accDev_t *acc) +{ + uint8_t data[6]; + + const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6); + if (!ack) { + return false; + } + + acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); + acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); + acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); + + return true; +} diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.h b/src/main/drivers/accgyro/accgyro_spi_icm20649.h new file mode 100644 index 0000000000..66739a8b0a --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.h @@ -0,0 +1,64 @@ +/* + * 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 "drivers/bus.h" + +#define ICM20649_BIT_RESET (0x80) + +#define ICM20649_RA_REG_BANK_SEL 0x7F + +// BANK 0 +#define ICM20649_RA_WHO_AM_I 0x00 +#define ICM20649_RA_PWR_MGMT_1 0x06 +#define ICM20649_RA_PWR_MGMT_2 0x07 +#define ICM20649_RA_INT_PIN_CFG 0x0F +#define ICM20649_RA_INT_ENABLE_1 0x11 +#define ICM20649_RA_GYRO_XOUT_H 0x33 +#define ICM20649_RA_ACCEL_XOUT_H 0x2D + +// BANK 2 +#define ICM20649_RA_GYRO_SMPLRT_DIV 0x00 +#define ICM20649_RA_GYRO_CONFIG_1 0x01 +#define ICM20649_RA_ACCEL_CONFIG 0x14 + +enum icm20649_gyro_fsr_e { + ICM20649_FSR_500DPS = 0, + ICM20649_FSR_1000DPS, + ICM20649_FSR_2000DPS, + ICM20649_FSR_4000DPS, + NUM_ICM20649_GYRO_FSR +}; + +enum icm20649_accel_fsr_e { + ICM20649_FSR_4G = 0, + ICM20649_FSR_8G, + ICM20649_FSR_16G, + ICM20649_FSR_30G, + NUM_ICM20649_ACCEL_FSR +}; + +void icm20649AccInit(accDev_t *acc); +void icm20649GyroInit(gyroDev_t *gyro); + +uint8_t icm20649SpiDetect(const busDevice_t *bus); + +bool icm20649SpiAccDetect(accDev_t *acc); +bool icm20649SpiGyroDetect(gyroDev_t *gyro); + +bool icm20649GyroReadSPI(gyroDev_t *gyro); +bool icm20649AccRead(accDev_t *acc); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2b687885ce..27c4c56c57 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -200,6 +200,9 @@ void spiPreInit(void) #ifdef USE_GYRO_SPI_MPU9250 spiPreInitCs(IO_TAG(MPU9250_CS_PIN)); #endif +#ifdef USE_GYRO_SPI_ICM20649 + spiPreInitCs(IO_TAG(ICM20649_CS_PIN)); +#endif #ifdef USE_GYRO_SPI_ICM20689 spiPreInitCs(IO_TAG(ICM20689_CS_PIN)); #endif diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 20b9132b67..2270864da6 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -81,13 +81,15 @@ // sync with accelerationSensor_e const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE" + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20649", "ICM20689", + "BMI160", "FAKE" }; // sync with gyroSensor_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", + "BMI160", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(BARO) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d82d8d55e2..2bad22a038 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,6 +45,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -243,6 +244,17 @@ retry: } break; } +#endif + ; // fallthrough + case ACC_ICM20649: +#ifdef USE_ACC_SPI_ICM20649 + if (icm20649SpiAccDetect(dev)) { + accHardware = ACC_ICM20649; +#ifdef ACC_ICM20649_ALIGN + dev->accAlign = ACC_ICM20649_ALIGN; +#endif + break; + } #endif ; // fallthrough case ACC_ICM20689: diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b211abbb65..f583a841bc 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -36,6 +36,7 @@ typedef enum { ACC_ICM20601, ACC_ICM20602, ACC_ICM20608G, + ACC_ICM20649, ACC_ICM20689, ACC_BMI160, ACC_FAKE diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 92871f3c79..aee591e446 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -110,7 +111,8 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 4 @@ -258,6 +260,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) } #endif +#ifdef USE_GYRO_SPI_ICM20649 + case GYRO_ICM20649: + if (icm20649SpiGyroDetect(dev)) { + gyroHardware = GYRO_ICM20649; +#ifdef GYRO_ICM20649_ALIGN + dev->gyroAlign = GYRO_ICM20649_ALIGN; +#endif + break; + } +#endif + #ifdef USE_GYRO_SPI_ICM20689 case GYRO_ICM20689: if (icm20689SpiGyroDetect(dev)) { @@ -302,7 +315,8 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) static bool gyroInitSensor(gyroSensor_t *gyroSensor) { -#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) #if defined(MPU_INT_EXTI) gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index fd055ceddb..e684960957 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -35,6 +35,7 @@ typedef enum { GYRO_ICM20601, GYRO_ICM20602, GYRO_ICM20608G, + GYRO_ICM20649, GYRO_ICM20689, GYRO_BMI160, GYRO_FAKE From 28d67027552ff9506a41130903a82aa0629c851c Mon Sep 17 00:00:00 2001 From: Brian Balogh Date: Fri, 1 Sep 2017 10:59:39 -0400 Subject: [PATCH 035/138] fix MSP acc scaling for ICM20649 --- src/main/fc/fc_msp.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e1329859ca..282e03d4a3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -750,8 +750,10 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) if (acc.dev.acc_1G > 512*4) { scale = 8; - } else if (acc.dev.acc_1G >= 512) { + } else if (acc.dev.acc_1G > 512*2) { scale = 4; + } else if (acc.dev.acc_1G >= 512) { + scale = 2; } else { scale = 1; } From 12583dc0d78a42995466c4b700f404458f9ecca6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 2 Sep 2017 08:53:38 +0100 Subject: [PATCH 036/138] Unit test makefile fix --- src/test/Makefile | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index b9a960ef0d..cb5b3a6d05 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -268,6 +268,9 @@ USER_INCLUDE_DIR = $(USER_DIR) OBJECT_DIR = ../../obj/test +# Determine the OS +UNAME := $(shell uname -s) + # Use clang/clang++ by default CC := clang CXX := clang++ @@ -278,12 +281,15 @@ COMMON_FLAGS = \ -Wextra \ -Werror \ -ggdb3 \ - -pthread \ -O0 \ -DUNIT_TEST \ -isystem $(GTEST_DIR)/inc \ -MMD -MP +ifneq ($(UNAME), Darwin) +COMMON_FLAGS += -pthread +endif + # Flags passed to the C compiler. C_FLAGS = $(COMMON_FLAGS) \ -std=gnu99 @@ -298,8 +304,7 @@ COVERAGE_FLAGS := --coverage C_FLAGS += $(COVERAGE_FLAGS) CXX_FLAGS += $(COVERAGE_FLAGS) -# Determine the OS and set up the parameter group linker flags accordingly -UNAME := $(shell uname -s) +# Set up the parameter group linker flags according to OS ifeq ($(UNAME), Darwin) PG_FLAGS = -Wl,-map,$(OBJECT_DIR)/$@.map else From 6d69b053e313ff4e7c1a2894d48d35475222730a Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 3 Sep 2017 23:09:36 +1200 Subject: [PATCH 037/138] Fixed conflict between USE_MSP_UART and other UART defines. --- src/main/io/serial.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 1cc30d1d42..388318ecad 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -119,15 +119,6 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) serialConfig->portConfigs[0].functionMask = FUNCTION_MSP; -#if defined(USE_VCP) && defined(USE_MSP_UART) - if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) { - serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1); - if (uart1Config) { - uart1Config->functionMask = FUNCTION_MSP; - } - } -#endif - #ifdef SERIALRX_UART serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART); if (serialRxUartConfig) { @@ -142,6 +133,15 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) } #endif +#if defined(USE_VCP) && defined(USE_MSP_UART) + if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) { + serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1); + if (uart1Config) { + uart1Config->functionMask = FUNCTION_MSP; + } + } +#endif + serialConfig->reboot_character = 'R'; serialConfig->serial_update_rate_hz = 100; } From 5fef94b51bf313a514d6f5e97888b24f7163d533 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 3 Sep 2017 23:49:38 +0900 Subject: [PATCH 038/138] V2: Rotate ICM20608 90deg CW to match MPU6000 --- src/main/target/OMNIBUSF7/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 4e000e7c77..c09cce08e4 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -55,6 +55,8 @@ #define MPU6500_SPI_INSTANCE SPI3 #define GYRO_1_CS_PIN MPU6000_CS_PIN #define GYRO_0_CS_PIN MPU6500_CS_PIN +#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG #else #define MPU6000_CS_PIN SPI3_NSS_PIN #define MPU6000_SPI_INSTANCE SPI3 From 7616c68909879afa217edac4ff31e9c70a5868b4 Mon Sep 17 00:00:00 2001 From: Bryce Johnson Date: Sun, 3 Sep 2017 12:27:19 -0500 Subject: [PATCH 039/138] Workaround for blheli 16.63 issue of sometimes the motors not reversing. Not sure if it is blheli or betaflight. --- src/main/drivers/pwm_output.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 067be78fc3..d205d5fe63 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -384,7 +384,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command) case DSHOT_CMD_SAVE_SETTINGS: case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: - repeats = 10; + repeats = 20; break; default: repeats = 1; From a11108a6ff3565094f7bffd632047a6847aa8633 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 3 Sep 2017 16:36:00 +0200 Subject: [PATCH 040/138] Fix custom PID and rate profile initialisation for all remaining targets --- src/main/target/ALIENFLIGHTF1/config.c | 16 ++++++---- src/main/target/ALIENFLIGHTF3/config.c | 16 ++++++---- src/main/target/ALIENFLIGHTF4/config.c | 20 ++++++++----- src/main/target/ALIENFLIGHTNGF7/config.c | 20 ++++++++----- src/main/target/BEEBRAIN_V2/config.c | 16 ++++++---- src/main/target/COLIBRI/config.c | 9 ++++-- src/main/target/FF_PIKOBLX/config.c | 38 ++++++++++++++---------- src/main/target/MULTIFLITEPICO/config.c | 24 ++++++++++----- src/main/target/SPRACINGF3EVO/config.c | 16 ++++++---- src/main/target/YUPIF4/config.c | 20 ++++++++----- src/main/target/YUPIF7/config.c | 20 ++++++++----- 11 files changed, 134 insertions(+), 81 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 90d1be9599..f06309a309 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -49,12 +49,16 @@ void targetConfiguration(void) motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; } - pidProfilesMutable(0)->pid[PID_ROLL].P = 90; - pidProfilesMutable(0)->pid[PID_ROLL].I = 44; - pidProfilesMutable(0)->pid[PID_ROLL].D = 60; - pidProfilesMutable(0)->pid[PID_PITCH].P = 90; - pidProfilesMutable(0)->pid[PID_PITCH].I = 44; - pidProfilesMutable(0)->pid[PID_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 90; + pidProfile->pid[PID_ROLL].I = 44; + pidProfile->pid[PID_ROLL].D = 60; + pidProfile->pid[PID_PITCH].P = 90; + pidProfile->pid[PID_PITCH].I = 44; + pidProfile->pid[PID_PITCH].D = 60; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 8431e680f4..a6341c2eec 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -107,12 +107,16 @@ void targetConfiguration(void) pidConfigMutable()->pid_process_denom = 1; } - pidProfilesMutable(0)->pid[PID_ROLL].P = 90; - pidProfilesMutable(0)->pid[PID_ROLL].I = 44; - pidProfilesMutable(0)->pid[PID_ROLL].D = 60; - pidProfilesMutable(0)->pid[PID_PITCH].P = 90; - pidProfilesMutable(0)->pid[PID_PITCH].I = 44; - pidProfilesMutable(0)->pid[PID_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 90; + pidProfile->pid[PID_ROLL].I = 44; + pidProfile->pid[PID_ROLL].D = 60; + pidProfile->pid[PID_PITCH].P = 90; + pidProfile->pid[PID_PITCH].I = 44; + pidProfile->pid[PID_PITCH].D = 60; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index cabae587c0..b13c52b3d5 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -71,14 +71,18 @@ void targetConfiguration(void) featureSet(FEATURE_TELEMETRY); } - pidProfilesMutable(0)->pid[PID_ROLL].P = 53; - pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 52; - pidProfilesMutable(0)->pid[PID_PITCH].P = 53; - pidProfilesMutable(0)->pid[PID_PITCH].I = 45; - pidProfilesMutable(0)->pid[PID_PITCH].D = 52; - pidProfilesMutable(0)->pid[PID_YAW].P = 64; - pidProfilesMutable(0)->pid[PID_YAW].D = 18; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 53; + pidProfile->pid[PID_ROLL].I = 45; + pidProfile->pid[PID_ROLL].D = 52; + pidProfile->pid[PID_PITCH].P = 53; + pidProfile->pid[PID_PITCH].I = 45; + pidProfile->pid[PID_PITCH].D = 52; + pidProfile->pid[PID_YAW].P = 64; + pidProfile->pid[PID_YAW].D = 18; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 4fa4e376a7..a5feb582d9 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -70,14 +70,18 @@ void targetConfiguration(void) featureSet(FEATURE_TELEMETRY); } - pidProfilesMutable(0)->pid[FD_ROLL].P = 53; - pidProfilesMutable(0)->pid[FD_ROLL].I = 45; - pidProfilesMutable(0)->pid[FD_ROLL].D = 52; - pidProfilesMutable(0)->pid[FD_PITCH].P = 53; - pidProfilesMutable(0)->pid[FD_PITCH].I = 45; - pidProfilesMutable(0)->pid[FD_PITCH].D = 52; - pidProfilesMutable(0)->pid[FD_YAW].P = 64; - pidProfilesMutable(0)->pid[FD_YAW].D = 18; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[FD_ROLL].P = 53; + pidProfile->pid[FD_ROLL].I = 45; + pidProfile->pid[FD_ROLL].D = 52; + pidProfile->pid[FD_PITCH].P = 53; + pidProfile->pid[FD_PITCH].I = 45; + pidProfile->pid[FD_PITCH].D = 52; + pidProfile->pid[FD_YAW].P = 64; + pidProfile->pid[FD_YAW].D = 18; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/BEEBRAIN_V2/config.c b/src/main/target/BEEBRAIN_V2/config.c index 9c79a8e9f9..ada367f353 100755 --- a/src/main/target/BEEBRAIN_V2/config.c +++ b/src/main/target/BEEBRAIN_V2/config.c @@ -63,12 +63,16 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; #endif - pidProfilesMutable(0)->pid[PID_ROLL].P = 90; - pidProfilesMutable(0)->pid[PID_ROLL].I = 44; - pidProfilesMutable(0)->pid[PID_ROLL].D = 60; - pidProfilesMutable(0)->pid[PID_PITCH].P = 90; - pidProfilesMutable(0)->pid[PID_PITCH].I = 44; - pidProfilesMutable(0)->pid[PID_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 90; + pidProfile->pid[PID_ROLL].I = 44; + pidProfile->pid[PID_ROLL].D = 60; + pidProfile->pid[PID_PITCH].P = 90; + pidProfile->pid[PID_PITCH].I = 44; + pidProfile->pid[PID_PITCH].D = 60; + } *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index d1b1844fa6..697dbbda95 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -54,8 +54,13 @@ void targetConfiguration(void) //rcControlsConfigMutable()->yaw_deadband = 10; compassConfigMutable()->mag_hardware = 1; - controlRateProfilesMutable(0)->dynThrPID = 45; - controlRateProfilesMutable(0)->tpa_breakpoint = 1700; + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->dynThrPID = 45; + controlRateConfig->tpa_breakpoint = 1700; + } + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } #endif diff --git a/src/main/target/FF_PIKOBLX/config.c b/src/main/target/FF_PIKOBLX/config.c index a7a94f5e09..a1e71d6fa8 100644 --- a/src/main/target/FF_PIKOBLX/config.c +++ b/src/main/target/FF_PIKOBLX/config.c @@ -64,22 +64,30 @@ void targetConfiguration(void) #endif parseRcChannels("TAER1234", rxConfigMutable()); - pidProfilesMutable(0)->pid[PID_ROLL].P = 80; - pidProfilesMutable(0)->pid[PID_ROLL].I = 37; - pidProfilesMutable(0)->pid[PID_ROLL].D = 35; - pidProfilesMutable(0)->pid[PID_PITCH].P = 100; - pidProfilesMutable(0)->pid[PID_PITCH].I = 37; - pidProfilesMutable(0)->pid[PID_PITCH].D = 35; - pidProfilesMutable(0)->pid[PID_YAW].P = 180; - pidProfilesMutable(0)->pid[PID_YAW].D = 45; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - controlRateProfilesMutable(0)->rcRate8 = 100; - controlRateProfilesMutable(0)->rcYawRate8 = 100; - controlRateProfilesMutable(0)->rcExpo8 = 15; - controlRateProfilesMutable(0)->rcYawExpo8 = 15; - controlRateProfilesMutable(0)->rates[PID_ROLL] = 80; - controlRateProfilesMutable(0)->rates[PID_PITCH] = 80; - controlRateProfilesMutable(0)->rates[PID_YAW] = 80; + pidProfile->pid[PID_ROLL].P = 80; + pidProfile->pid[PID_ROLL].I = 37; + pidProfile->pid[PID_ROLL].D = 35; + pidProfile->pid[PID_PITCH].P = 100; + pidProfile->pid[PID_PITCH].I = 37; + pidProfile->pid[PID_PITCH].D = 35; + pidProfile->pid[PID_YAW].P = 180; + pidProfile->pid[PID_YAW].D = 45; + } + + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 100; + controlRateConfig->rcExpo8 = 15; + controlRateConfig->rcYawExpo8 = 15; + controlRateConfig->rates[PID_ROLL] = 80; + controlRateConfig->rates[PID_PITCH] = 80; + controlRateConfig->rates[PID_YAW] = 80; + } } } #endif diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index aff23afdea..d1814fbb69 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -77,14 +77,22 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_sync_denom = 4; pidConfigMutable()->pid_process_denom = 1; - pidProfilesMutable(0)->pid[PID_ROLL].P = 70; - pidProfilesMutable(0)->pid[PID_ROLL].I = 62; - pidProfilesMutable(0)->pid[PID_ROLL].D = 19; - pidProfilesMutable(0)->pid[PID_PITCH].P = 70; - pidProfilesMutable(0)->pid[PID_PITCH].I = 62; - pidProfilesMutable(0)->pid[PID_PITCH].D = 19; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - controlRateProfilesMutable(0)->rcRate8 = 70; - pidProfilesMutable(0)->pid[PID_LEVEL].I = 40; + pidProfile->pid[PID_ROLL].P = 70; + pidProfile->pid[PID_ROLL].I = 62; + pidProfile->pid[PID_ROLL].D = 19; + pidProfile->pid[PID_PITCH].P = 70; + pidProfile->pid[PID_PITCH].I = 62; + pidProfile->pid[PID_PITCH].D = 19; + pidProfile->pid[PID_LEVEL].I = 40; + } + + for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { + controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); + + controlRateConfig->rcRate8 = 70; + } } #endif diff --git a/src/main/target/SPRACINGF3EVO/config.c b/src/main/target/SPRACINGF3EVO/config.c index 91d897aa91..6d9231ce1a 100644 --- a/src/main/target/SPRACINGF3EVO/config.c +++ b/src/main/target/SPRACINGF3EVO/config.c @@ -44,12 +44,16 @@ void targetConfiguration(void) motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - pidProfilesMutable(0)->pid[FD_ROLL].P = 90; - pidProfilesMutable(0)->pid[FD_ROLL].I = 44; - pidProfilesMutable(0)->pid[FD_ROLL].D = 60; - pidProfilesMutable(0)->pid[FD_PITCH].P = 90; - pidProfilesMutable(0)->pid[FD_PITCH].I = 44; - pidProfilesMutable(0)->pid[FD_PITCH].D = 60; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[FD_ROLL].P = 90; + pidProfile->pid[FD_ROLL].I = 44; + pidProfile->pid[FD_ROLL].D = 60; + pidProfile->pid[FD_PITCH].P = 90; + pidProfile->pid[FD_PITCH].I = 44; + pidProfile->pid[FD_PITCH].D = 60; + } #endif } #endif diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index a72a10e1f2..15d5ce0771 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -47,13 +47,17 @@ void targetConfiguration(void) } /* Specific PID values for YupiF4 */ - pidProfilesMutable(0)->pid[PID_ROLL].P = 30; - pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 20; - pidProfilesMutable(0)->pid[PID_PITCH].P = 30; - pidProfilesMutable(0)->pid[PID_PITCH].I = 50; - pidProfilesMutable(0)->pid[PID_PITCH].D = 20; - pidProfilesMutable(0)->pid[PID_YAW].P = 40; - pidProfilesMutable(0)->pid[PID_YAW].I = 50; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 30; + pidProfile->pid[PID_ROLL].I = 45; + pidProfile->pid[PID_ROLL].D = 20; + pidProfile->pid[PID_PITCH].P = 30; + pidProfile->pid[PID_PITCH].I = 50; + pidProfile->pid[PID_PITCH].D = 20; + pidProfile->pid[PID_YAW].P = 40; + pidProfile->pid[PID_YAW].I = 50; + } } #endif diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/YUPIF7/config.c index 7d1a6100c0..b83870d235 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/YUPIF7/config.c @@ -30,13 +30,17 @@ void targetConfiguration(void) { /* Specific PID values for YupiF4 */ - pidProfilesMutable(0)->pid[PID_ROLL].P = 30; - pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 20; - pidProfilesMutable(0)->pid[PID_PITCH].P = 30; - pidProfilesMutable(0)->pid[PID_PITCH].I = 50; - pidProfilesMutable(0)->pid[PID_PITCH].D = 20; - pidProfilesMutable(0)->pid[PID_YAW].P = 40; - pidProfilesMutable(0)->pid[PID_YAW].I = 50; + for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { + pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); + + pidProfile->pid[PID_ROLL].P = 30; + pidProfile->pid[PID_ROLL].I = 45; + pidProfile->pid[PID_ROLL].D = 20; + pidProfile->pid[PID_PITCH].P = 30; + pidProfile->pid[PID_PITCH].I = 50; + pidProfile->pid[PID_PITCH].D = 20; + pidProfile->pid[PID_YAW].P = 40; + pidProfile->pid[PID_YAW].I = 50; + } } #endif From 1d3a2f253ec836278ed977277a873d70eb1b2306 Mon Sep 17 00:00:00 2001 From: Steffen Windoffer Date: Wed, 30 Aug 2017 10:03:12 +0200 Subject: [PATCH 041/138] fix crsf mah telemetry --- src/main/telemetry/crsf.c | 10 +++++----- src/test/unit/telemetry_crsf_unittest.cc | 7 ++++++- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 6d041ebcd6..79d2357cbc 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -135,7 +135,7 @@ void crsfFrameGps(sbuf_t *dst) Payload: uint16_t Voltage ( mV * 100 ) uint16_t Current ( mA * 100 ) -uint24_t Capacity ( mAh ) +uint24_t Fuel ( drawn mAh ) uint8_t Battery remaining ( percent ) */ void crsfFrameBatterySensor(sbuf_t *dst) @@ -145,11 +145,11 @@ void crsfFrameBatterySensor(sbuf_t *dst) sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.1V sbufWriteU16BigEndian(dst, getAmperage() / 10); - const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; + const uint32_t mAhDrawn = getMAhDrawn(); const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining(); - sbufWriteU8(dst, (batteryCapacity >> 16)); - sbufWriteU8(dst, (batteryCapacity >> 8)); - sbufWriteU8(dst, (uint8_t)batteryCapacity); + sbufWriteU8(dst, (mAhDrawn >> 16)); + sbufWriteU8(dst, (mAhDrawn >> 8)); + sbufWriteU8(dst, (uint8_t)mAhDrawn); sbufWriteU8(dst, batteryRemainingPercentage); } diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index a14e00bc0b..2f92636d8c 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -58,6 +58,7 @@ extern "C" { uint16_t testBatteryVoltage = 0; int32_t testAmperage = 0; + int32_t testmAhDrawn = 0; serialPort_t *telemetrySharedPort; PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); @@ -155,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery) testBatteryVoltage = 33; // 3.3V = 3300 mv testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps - batteryConfigMutable()->batteryCapacity = 1234; + testmAhDrawn = 1234; frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); @@ -318,4 +319,8 @@ uint8_t calculateBatteryPercentageRemaining(void) { return 67; } +int32_t getMAhDrawn(void){ + return testmAhDrawn; +} + } From 4b2e3c6ddafb05ddad64457442bec2f92188f6fb Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 5 Sep 2017 08:30:03 +1200 Subject: [PATCH 042/138] Fixed Dshot command issues with BLHeli_S ESCs. --- src/main/drivers/pwm_output.c | 2 +- src/main/fc/fc_core.c | 4 ++++ src/main/io/beeper.c | 4 ++++ 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d205d5fe63..067be78fc3 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -384,7 +384,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command) case DSHOT_CMD_SAVE_SETTINGS: case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: - repeats = 20; + repeats = 10; break; default: repeats = 1; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0cd9933e9a..0a90d5a73f 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -262,6 +262,8 @@ void tryArm(void) } #ifdef USE_DSHOT if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXDSHOTREVERSE)) { + pwmDisableMotors(); + if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) { reverseMotors = false; pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL); @@ -269,6 +271,8 @@ void tryArm(void) reverseMotors = true; pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED); } + + pwmEnableMotors(); } #endif diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index cb2a9430bb..a51cb925cd 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -364,7 +364,11 @@ void beeperUpdate(timeUs_t currentTimeUs) #ifdef USE_DSHOT if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { + pwmDisableMotors(); + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3); + + pwmEnableMotors(); } #endif From 420ed2d0a3edbf9388615ce8a54409edc3e0dc1d Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 5 Sep 2017 21:08:09 +1200 Subject: [PATCH 043/138] Added halfduplex option for VTX control ports. --- src/main/fc/settings.c | 8 +++++++- src/main/io/vtx_control.c | 7 ++++++- src/main/io/vtx_control.h | 1 + src/main/io/vtx_smartaudio.c | 10 +++++++++- src/main/io/vtx_tramp.c | 12 +++++++++--- src/main/target/SPRACINGF3EVO/target.h | 1 + 6 files changed, 33 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 49063490c7..51906b31ca 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -60,6 +60,7 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/osd.h" +#include "io/vtx_control.h" #include "io/vtx_rtc6705.h" #include "rx/rx.h" @@ -715,13 +716,18 @@ const clivalue_t valueTable[] = { #endif { "pwr_on_arm_grace", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, powerOnArmingGraceTime) }, -// PG_VTX_CONFIG +// PG_VTX_RTC6705_CONFIG #ifdef VTX_RTC6705 { "vtx_band", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, band) }, { "vtx_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 8 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, channel) }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, RTC6705_POWER_COUNT - 1 }, PG_VTX_RTC6705_CONFIG, offsetof(vtxRTC6705Config_t, power) }, #endif +// PG_VTX_CONFIG +#if defined(VTX_CONTROL) && defined(VTX_COMMON) + { "vtx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) }, +#endif + // PG_VCD_CONFIG #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 2 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) }, diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 622f656e70..eb78d18781 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -40,7 +40,12 @@ #if defined(VTX_CONTROL) && defined(VTX_COMMON) -PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); + +PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, +// .vtxChannelActivationConditions = { 0 }, + .halfDuplex = true +); static uint8_t locked = 0; diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 3715b60e00..9c1e19eb8a 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -30,6 +30,7 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; + uint8_t halfDuplex; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index ea293f39c4..1fadb13685 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -49,6 +49,7 @@ #include "flight/pid.h" #include "io/serial.h" +#include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_string.h" @@ -622,7 +623,14 @@ bool vtxSmartAudioInit() serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + portOptions_e portOptions = 0; +#if defined(VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, 4800, MODE_RXTX, portOptions); } if (!smartAudioSerialPort) { diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 281eb003d1..73eaef16db 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -35,11 +35,10 @@ #include "drivers/vtx_common.h" #include "io/serial.h" +#include "io/vtx_control.h" #include "io/vtx_string.h" #include "io/vtx_tramp.h" -#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) - #if defined(CMS) || defined(VTX_COMMON) const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { 25, 100, 200, 400, 600 @@ -518,7 +517,14 @@ bool vtxTrampInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + portOptions_e portOptions = 0; +#if defined(VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, portOptions); } if (!trampSerialPort) { diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 7188b66211..ffc531b982 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -41,6 +41,7 @@ #undef USE_UNCOMMON_MIXERS #endif #undef TELEMETRY_JETIEXBUS +#undef USE_SERIALRX_JETIEXBUS #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT From ad7da28b1dbe009a9396971cba08b6ba439d86ce Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 6 Sep 2017 12:10:27 +0200 Subject: [PATCH 044/138] Change Default telemetry port BFF4 --- src/main/target/BETAFLIGHTF4/target.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index ef76e13421..10b5168af8 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -123,11 +123,11 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART2 -#define SBUS_TELEMETRY_UART SERIAL_PORT_SOFTSERIAL1 +#define SERIALRX_UART SERIAL_PORT_USART6 +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART2 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_SOFTSERIAL) +#define DEFAULT_FEATURES ( FEATURE_TELEMETRY | FEATURE_OSD ) #define SPEKTRUM_BIND // USART3, From 913d3a9461e9c5029da83e0f614909191cf4a7d5 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Thu, 7 Sep 2017 00:36:04 -0400 Subject: [PATCH 045/138] ALIENWHOOP add support for OSD on upcoming V3 board --- src/main/target/ALIENWHOOP/target.h | 15 +++++++++++++-- src/main/target/ALIENWHOOP/target.mk | 3 ++- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/target/ALIENWHOOP/target.h b/src/main/target/ALIENWHOOP/target.h index eebdb91099..921ed5e538 100644 --- a/src/main/target/ALIENWHOOP/target.h +++ b/src/main/target/ALIENWHOOP/target.h @@ -104,6 +104,15 @@ //#define SPI5_MOSI_PIN #endif +/* OSD MAX7456E */ +#define OSD + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + /* Motion Processing Unit (MPU) - Invensense 6-axis MPU-6500 or 9-axis MPU-9250 */ // Interrupt @@ -132,11 +141,13 @@ /* Optional Digital Pressure Sensor (barometer) - Bosch BMP280 * TODO: not implemented on V1 or V2 pcb */ +#if defined(BREADBOARD) #define BARO #define USE_BARO_BMP280 #define USE_BARO_SPI_BMP280 #define BMP280_SPI_INSTANCE SPI3 #define BMP280_CS_PIN SPI3_NSS_PIN +#endif /* Serial ports etc. */ @@ -185,9 +196,9 @@ /* Defaults - What do we want out of the box? */ #if defined(BREADBOARD) -#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP | FEATURE_LED_STRIP ) +#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP | FEATURE_LED_STRIP | FEATURE_OSD ) #else -#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP) // FEATURE_TELEMETRY changes bind pin from rx to tx +#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP ) // TODO FEATURE_OSD for V3 board ... FEATURE_TELEMETRY changes bind pin from rx to tx #endif #undef VTX_COMMON diff --git a/src/main/target/ALIENWHOOP/target.mk b/src/main/target/ALIENWHOOP/target.mk index 62476322ab..39fe6f76ae 100644 --- a/src/main/target/ALIENWHOOP/target.mk +++ b/src/main/target/ALIENWHOOP/target.mk @@ -21,4 +21,5 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ - drivers/compass/compass_ak8963.c + drivers/compass/compass_ak8963.c \ + drivers/max7456.c From 723b71a4d0be34fa87da632c8f7855986b090317 Mon Sep 17 00:00:00 2001 From: jirif Date: Sun, 3 Sep 2017 17:57:22 +0200 Subject: [PATCH 046/138] BF 3.2 setpointweight is disabled by default fix for https://github.com/betaflight/betaflight/issues/4017 --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 842d172094..c3ce0798b9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -489,7 +489,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered); float dynC = 0; - if ( (pidProfile->setpointRelaxRatio < 100) && (!flightModeFlags) ) { + if ( (pidProfile->dtermSetpointWeight > 0) && (!flightModeFlags) ) { dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y From c9b06c5bfe08104411d7c4a08a29dba5a6e62409 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 7 Sep 2017 11:47:43 +0100 Subject: [PATCH 047/138] Undefined USE_SERVOS for BEEBRAIN to save ROM --- src/main/target/NAZE/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 4c24d3f375..28f2d9c452 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -36,6 +36,7 @@ #define TARGET_BOARD_IDENTIFIER "AFMN" #elif defined(BEEBRAIN) #define BRUSHED_MOTORS +#undef USE_SERVOS #define TARGET_BOARD_IDENTIFIER "BEBR" #define TARGET_CONFIG #define DEFAULT_FEATURES FEATURE_MOTOR_STOP From 49afecdcb203fed9f4153c157ed3a2e583b36a8f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 7 Sep 2017 11:52:35 +0100 Subject: [PATCH 048/138] Undefined TELEMETRY_JETIEXBUS for STM32F3DISCOVERY to save ROM --- src/main/target/STM32F3DISCOVERY/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 6eabdeeadc..0ff719e5d0 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -31,6 +31,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE +#undef TELEMETRY_JETIEXBUS // ROM SAVING + + #define CURRENT_TARGET_CPU_VOLTAGE 3.0 #define LED0_PIN PE8 // Blue LEDs - PE8/PE12 From fd37566bc59829f752834cc2ce39be96dfbf2d05 Mon Sep 17 00:00:00 2001 From: Bryce Johnson Date: Fri, 18 Aug 2017 17:00:24 -0500 Subject: [PATCH 049/138] New turtle mode that only spins needed props --- src/main/fc/fc_core.c | 16 +++++++++++----- src/main/fc/fc_core.h | 1 + src/main/fc/fc_msp_box.c | 6 +++--- src/main/fc/rc_modes.h | 2 +- src/main/flight/mixer.c | 33 +++++++++++++++++++++++++++++++++ 5 files changed, 49 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0a90d5a73f..6a37dadaae 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -109,6 +109,8 @@ int16_t magHold; int16_t headFreeModeHold; static bool reverseMotors = false; +static bool flipOverAfterCrashMode = false; + static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; @@ -261,14 +263,14 @@ void tryArm(void) return; } #ifdef USE_DSHOT - if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXDSHOTREVERSE)) { + if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { pwmDisableMotors(); - if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) { - reverseMotors = false; + if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + flipOverAfterCrashMode = false; pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL); } else { - reverseMotors = true; + flipOverAfterCrashMode = true; pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED); } @@ -723,7 +725,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } } -bool isMotorsReversed() +bool isMotorsReversed(void) { return reverseMotors; } +bool isFlipOverAfterCrashMode(void) +{ + return flipOverAfterCrashMode; +} diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 085ce3d758..d9ac269c69 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -49,3 +49,4 @@ void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); bool isMotorsReversed(void); +bool isFlipOverAfterCrashMode(void); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 769d1c7e13..ee87442e73 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -76,7 +76,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXCAMERA1, "CAMERA CONTROL 1", 32}, { BOXCAMERA2, "CAMERA CONTROL 2", 33}, { BOXCAMERA3, "CAMERA CONTROL 3", 34 }, - { BOXDSHOTREVERSE, "DSHOT REVERSE MOTORS", 35 }, + { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH (DSHOT ONLY)", 35 }, { BOXPREARM, "PREARM", 36 }, }; @@ -217,7 +217,7 @@ void initActiveBoxIds(void) } if (isMotorProtocolDshot()) { - BME(BOXDSHOTREVERSE); + BME(BOXFLIPOVERAFTERCRASH); } if (feature(FEATURE_SERVO_TILT)) { @@ -290,7 +290,7 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) const uint64_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON) | BM(BOXLEDMAX) | BM(BOXLEDLOW) | BM(BOXLLIGHTS) | BM(BOXCALIB) | BM(BOXGOV) | BM(BOXOSD) | BM(BOXTELEMETRY) | BM(BOXGTUNE) | BM(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE) - | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXDSHOTREVERSE) | BM(BOX3DDISABLE); + | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX) | BM(BOXFLIPOVERAFTERCRASH) | BM(BOX3DDISABLE); STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes); for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) { if ((rcModeCopyMask & BM(i)) // mode copy is enabled diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 25658e9fdb..500103c4f2 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -57,7 +57,7 @@ typedef enum { BOXCAMERA1, BOXCAMERA2, BOXCAMERA3, - BOXDSHOTREVERSE, + BOXFLIPOVERAFTERCRASH, BOXPREARM, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 0cf6465d42..73c8af4b6d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -43,6 +43,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/fc_core.h" +#include "fc/fc_rc.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -563,6 +564,34 @@ void calculateThrottleAndCurrentMotorEndpoints(void) motorOutputRange = motorOutputMax - motorOutputMin; } +static void applyFlipOverAfterCrashModeToMotors(void) +{ + float motorMix[MAX_SUPPORTED_MOTORS]; + + for (int i = 0; i < motorCount; i++) { + if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { + motorMix[i] = getRcDeflection(FD_ROLL) * pidSumLimit * currentMixer[i].roll * (-1); + } else { + motorMix[i] = getRcDeflection(FD_PITCH) * pidSumLimit * currentMixer[i].pitch * (-1); + } + } + // Apply the mix to motor endpoints + for (uint32_t i = 0; i < motorCount; i++) { + float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i]); + //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered + motorOutput = (motorOutput < motorOutputMin + 20 ) ? disarmMotorOutput : motorOutput; + + motor[i] = motorOutput; + } + + // Disarmed mode + if (!ARMING_FLAG(ARMED)) { + for (int i = 0; i < motorCount; i++) { + motor[i] = motor_disarmed[i]; + } + } +} + static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted @@ -604,6 +633,10 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) void mixTable(uint8_t vbatPidCompensation) { + if (isFlipOverAfterCrashMode()) { + applyFlipOverAfterCrashModeToMotors(); + return; + } // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(); From b295a159b0d1dfc158d5cba642f3a2148ed3b12e Mon Sep 17 00:00:00 2001 From: brianlbalogh Date: Thu, 7 Sep 2017 09:05:11 -0400 Subject: [PATCH 050/138] Add gyro rates and configurable FSR for ICM20649 (#4077) --- src/main/drivers/accgyro/accgyro.h | 4 +++ .../drivers/accgyro/accgyro_spi_icm20649.c | 15 ++++----- src/main/drivers/gyro_sync.c | 33 ++++++++++++++----- src/main/fc/config.c | 22 +++++++++++-- src/main/fc/settings.c | 6 ++++ src/main/sensors/acceleration.c | 4 ++- src/main/sensors/acceleration.h | 1 + src/main/sensors/gyro.c | 2 ++ src/main/sensors/gyro.h | 1 + 9 files changed, 67 insertions(+), 21 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 4900638e67..eece833027 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -42,8 +42,10 @@ typedef enum { GYRO_RATE_1_kHz, + GYRO_RATE_1100_Hz, GYRO_RATE_3200_Hz, GYRO_RATE_8_kHz, + GYRO_RATE_9_kHz, GYRO_RATE_32_kHz, } gyroRateKHz_e; @@ -70,6 +72,7 @@ typedef struct gyroDev_s { mpuDetectionResult_t mpuDetectionResult; ioTag_t mpuIntExtiTag; mpuConfiguration_t mpuConfiguration; + bool gyro_high_fsr; } gyroDev_t; typedef struct accDev_s { @@ -86,6 +89,7 @@ typedef struct accDev_s { sensor_align_e accAlign; mpuDetectionResult_t mpuDetectionResult; mpuConfiguration_t mpuConfiguration; + bool acc_high_fsr; } accDev_t; static inline void accDevLock(accDev_t *acc) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index fb0030951d..e6210c4bbe 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -35,9 +35,6 @@ #include "accgyro_mpu.h" #include "accgyro_spi_icm20649.h" -static bool use4kDps = true; // TODO: make these configurable for testing -static bool use30g = true; - static void icm20649SpiInit(const busDevice_t *bus) { static bool hardwareInitialised = false; @@ -75,7 +72,7 @@ uint8_t icm20649SpiDetect(const busDevice_t *bus) const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I); if (whoAmI == ICM20649_WHO_AM_I_CONST) { icmDetected = ICM_20649_SPI; - } else { + } else { icmDetected = MPU_NONE; } if (icmDetected != MPU_NONE) { @@ -94,13 +91,13 @@ void icm20649AccInit(accDev_t *acc) { // 2,048 LSB/g 16g // 1,024 LSB/g 30g - acc->acc_1G = use30g ? 1024 : 2048; + acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048; spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 delay(15); - const uint8_t acc_fsr = use30g ? ICM20649_FSR_30G : ICM20649_FSR_16G; + const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G; acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); delay(15); acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 @@ -134,8 +131,8 @@ void icm20649GyroInit(gyroDev_t *gyro) delay(15); gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 delay(15); - const uint8_t gyro_fsr = use4kDps ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; - uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1_kHz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) + const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; + uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3; gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); delay(15); @@ -164,7 +161,7 @@ bool icm20649SpiGyroDetect(gyroDev_t *gyro) // 16.4 dps/lsb 2kDps // 8.2 dps/lsb 4kDps - gyro->scale = 1.0f / (use4kDps ? 8.2f : 16.4f); + gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f); return true; } diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index c89a1ba95e..e32754cec8 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -36,17 +36,32 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin gyro->gyroRateKHz = GYRO_RATE_32_kHz; gyroSamplePeriod = 31.5f; } else { -#ifdef USE_ACCGYRO_BMI160 - gyro->gyroRateKHz = GYRO_RATE_3200_Hz; - gyroSamplePeriod = 312.0f; -#else - gyro->gyroRateKHz = GYRO_RATE_8_kHz; - gyroSamplePeriod = 125.0f; -#endif + switch (gyro->mpuDetectionResult.sensor) { + case BMI_160_SPI: + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + gyroSamplePeriod = 312.0f; + break; + case ICM_20649_SPI: + gyro->gyroRateKHz = GYRO_RATE_9_kHz; + gyroSamplePeriod = 1000000.0f / 9000.0f; + break; + default: + gyro->gyroRateKHz = GYRO_RATE_8_kHz; + gyroSamplePeriod = 125.0f; + break; + } } } else { - gyro->gyroRateKHz = GYRO_RATE_1_kHz; - gyroSamplePeriod = 1000.0f; + switch (gyro->mpuDetectionResult.sensor) { + case ICM_20649_SPI: + gyro->gyroRateKHz = GYRO_RATE_1100_Hz; + gyroSamplePeriod = 1000000.0f / 1100.0f; + break; + default: + gyro->gyroRateKHz = GYRO_RATE_1_kHz; + gyroSamplePeriod = 1000.0f; + break; + } gyroSyncDenominator = 1; // Always full Sampling 1khz } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 007596ab27..21d6e5ae1b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -507,13 +507,31 @@ void validateAndFixGyroConfig(void) gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } - float samplingTime = 0.000125f; + float samplingTime; + switch (gyroMpuDetectionResult()->sensor) { + case ICM_20649_SPI: + samplingTime = 1.0f / 9000.0f; + break; + case BMI_160_SPI: + samplingTime = 0.0003125f; + break; + default: + samplingTime = 0.000125f; + break; + } if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed gyroConfigMutable()->gyro_sync_denom = 1; gyroConfigMutable()->gyro_use_32khz = false; - samplingTime = 0.001f; + switch (gyroMpuDetectionResult()->sensor) { + case ICM_20649_SPI: + samplingTime = 1.0f / 1100.0f; + break; + default: + samplingTime = 0.001f; + break; + } } if (gyroConfig()->gyro_use_32khz) { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 51906b31ca..8ff270544c 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -301,6 +301,9 @@ const clivalue_t valueTable[] = { // PG_GYRO_CONFIG { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf) }, +#if defined(USE_GYRO_SPI_ICM20649) + { "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) }, +#endif { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) }, { "gyro_lowpass_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) }, @@ -321,6 +324,9 @@ const clivalue_t valueTable[] = { // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) }, +#if defined(USE_GYRO_SPI_ICM20649) + { "acc_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_high_fsr) }, +#endif { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) }, { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) }, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 2bad22a038..bd33f8674a 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -113,7 +113,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) RESET_CONFIG_2(accelerometerConfig_t, instance, .acc_lpf_hz = 10, .acc_align = ALIGN_DEFAULT, - .acc_hardware = ACC_DEFAULT + .acc_hardware = ACC_DEFAULT, + .acc_high_fsr = false, ); resetRollAndPitchTrims(&instance->accelerometerTrims); resetFlightDynamicsTrims(&instance->accZero); @@ -317,6 +318,7 @@ bool accInit(uint32_t gyroSamplingInverval) acc.dev.bus = *gyroSensorBus(); acc.dev.mpuConfiguration = *gyroMpuConfiguration(); acc.dev.mpuDetectionResult = *gyroMpuDetectionResult(); + acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr; if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index f583a841bc..57ee0fc82d 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -66,6 +66,7 @@ typedef struct accelerometerConfig_s { uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz sensor_align_e acc_align; // acc alignment uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device + bool acc_high_fsr; flightDynamicsTrims_t accZero; rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index aee591e446..74fff590bd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -127,6 +127,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_256HZ, .gyro_soft_lpf_type = FILTER_PT1, .gyro_soft_lpf_hz = 90, + .gyro_high_fsr = false, .gyro_use_32khz = false, .gyro_to_use = 0, .gyro_soft_notch_hz_1 = 400, @@ -335,6 +336,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) mpuDetect(&gyroSensor->gyroDev); mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif + gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); if (gyroHardware == GYRO_NONE) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e684960957..801fe6bc9d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -55,6 +55,7 @@ typedef struct gyroConfig_s { uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_type; uint8_t gyro_soft_lpf_hz; + bool gyro_high_fsr; bool gyro_use_32khz; uint8_t gyro_to_use; uint16_t gyro_soft_notch_hz_1; From 13308883b2152518db72ed43330c0cdaec66f62e Mon Sep 17 00:00:00 2001 From: gke Date: Thu, 7 Sep 2017 23:07:07 +1000 Subject: [PATCH 051/138] Yaw rate change limiter solution for YTTM (#4058) --- src/main/drivers/accgyro/accgyro.h | 1 + src/main/sensors/gyro.c | 45 +++++++++++++++++++++++++++--- 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index eece833027..f6df5875a0 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -60,6 +60,7 @@ typedef struct gyroDev_s { int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment int16_t gyroADCRaw[XYZ_AXIS_COUNT]; + int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int16_t temperature; uint8_t lpf; gyroRateKHz_e gyroRateKHz; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 74fff590bd..8a8c89c6d1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -19,6 +19,7 @@ #include #include #include +#include #include "platform.h" @@ -69,6 +70,10 @@ #include "hardware_revision.h" #endif +#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689))) +#define USE_GYRO_SLEW_LIMITER +#endif + gyro_t gyro; static uint8_t gyroDebugMode; @@ -365,6 +370,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align; } + gyroInitSensorFilters(gyroSensor); #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyseInit(gyro.targetLooptime); @@ -437,6 +443,14 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch return notchHz; } +#if defined(USE_GYRO_SLEW_LIMITER) +void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) { + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; +} +#endif + static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) { gyroSensor->notchFilter1ApplyFn = nullFilterApply; @@ -484,6 +498,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { +#if defined(USE_GYRO_SLEW_LIMITER) + gyroInitSlewLimiter(gyroSensor); +#endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); @@ -582,6 +599,21 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t } +#if defined(USE_GYRO_SLEW_LIMITER) +int32_t gyroSlewLimiter(int axis, gyroSensor_t *gyroSensor) +{ + int32_t newRawGyro = (int32_t) gyroSensor->gyroDev.gyroADCRaw[axis]; + + if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) + newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; + else + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + + return newRawGyro; +} +#endif + + void gyroUpdateSensor(gyroSensor_t *gyroSensor) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -590,10 +622,15 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { - // move gyro data into 32-bit variables to avoid overflows in calculations - gyroSensor->gyroDev.gyroADC[X] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[X] - (int32_t)gyroSensor->gyroDev.gyroZero[X]; - gyroSensor->gyroDev.gyroADC[Y] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Y] - (int32_t)gyroSensor->gyroDev.gyroZero[Y]; - gyroSensor->gyroDev.gyroADC[Z] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Z] - (int32_t)gyroSensor->gyroDev.gyroZero[Z]; + // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations + + gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; + gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; +#if defined(USE_GYRO_SLEW_LIMITER) + gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(Z, gyroSensor) - gyroSensor->gyroDev.gyroZero[Z]; +#else + gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; +#endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else { From f8c9b57fd6f7c923a03ab85e9ae596d91be4bcf2 Mon Sep 17 00:00:00 2001 From: cs8425 Date: Fri, 8 Sep 2017 00:12:12 +0800 Subject: [PATCH 052/138] fix cli crash on get, dump, diff on SITL --- make/mcu/SITL.mk | 1 - src/main/config/config_eeprom.c | 6 +----- src/main/config/config_streamer.c | 2 ++ src/main/target/SITL/target.c | 6 ------ src/main/target/SITL/target.h | 11 ++++++++++- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/make/mcu/SITL.mk b/make/mcu/SITL.mk index 242b42450e..90210fa1a0 100644 --- a/make/mcu/SITL.mk +++ b/make/mcu/SITL.mk @@ -25,7 +25,6 @@ MCU_EXCLUDES = \ drivers/dma.c \ drivers/pwm_output.c \ drivers/timer.c \ - drivers/light_led.c \ drivers/system.c \ drivers/rcc.c \ drivers/serial_escserial.c \ diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index f784725e0d..702e15eeb6 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -33,11 +33,7 @@ #include "drivers/system.h" -#ifdef EEPROM_IN_RAM -extern uint8_t eepromData[EEPROM_SIZE]; -# define __config_start (*eepromData) -# define __config_end (*ARRAYEND(eepromData)) -#else +#ifndef EEPROM_IN_RAM extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; #endif diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index 14c800d906..1fb05a0350 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -23,8 +23,10 @@ #include "config/config_streamer.h" +#ifndef EEPROM_IN_RAM extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; +#endif #if !defined(FLASH_PAGE_SIZE) // F1 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 22738ba6a7..70d2d872dc 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -254,12 +254,6 @@ void systemResetToBootloader(void) { exit(0); } -// drivers/light_led.c -void ledInit(const statusLedConfig_t *statusLedConfig) { - UNUSED(statusLedConfig); - printf("[led]Init...\n"); -} - void timerInit(void) { printf("[timer]Init...\n"); } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 0ab64e0821..9bb9b4a327 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -35,7 +35,7 @@ // file name to save config #define EEPROM_FILENAME "eeprom.bin" #define EEPROM_IN_RAM -#define EEPROM_SIZE 8192 +#define EEPROM_SIZE 32768 #define U_ID_0 0 #define U_ID_1 1 @@ -137,6 +137,15 @@ uint32_t SystemCoreClock; +#ifdef EEPROM_IN_RAM +extern uint8_t eepromData[EEPROM_SIZE]; +#define __config_start (*eepromData) +#define __config_end (*ARRAYEND(eepromData)) +#else +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; +#endif + #define UNUSED(x) (void)(x) typedef enum From c97c8998b178d15c146576b107dbc50658d23b2f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 7 Sep 2017 21:35:37 +0100 Subject: [PATCH 053/138] Update PG version numbers --- src/main/fc/config.c | 2 +- src/main/sensors/battery.c | 4 ++-- src/main/sensors/battery.h | 2 +- src/main/sensors/gyro.c | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 007596ab27..c98325151d 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -124,7 +124,7 @@ PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, .name = { 0 } ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 1); #ifndef USE_OSD_SLAVE #if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 97254497a6..a0320b3cd4 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -87,7 +87,7 @@ static batteryState_e consumptionState; #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE #endif -PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 2); PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, // voltage @@ -265,7 +265,7 @@ void batteryUpdateStates(timeUs_t currentTimeUs) batteryState = MAX(voltageState, consumptionState); } -lowVoltageCutoff_t *getLowVoltageCutoff(void) +const lowVoltageCutoff_t *getLowVoltageCutoff(void) { return &lowVoltageCutoff; } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 770c6b3759..467b229329 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -87,4 +87,4 @@ int32_t getMAhDrawn(void); void batteryUpdateCurrentMeter(timeUs_t currentTimeUs); -lowVoltageCutoff_t *getLowVoltageCutoff(void); +const lowVoltageCutoff_t *getLowVoltageCutoff(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index aee591e446..91e0f0e05a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -118,7 +118,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #define GYRO_SYNC_DENOM_DEFAULT 4 #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = ALIGN_DEFAULT, From 534b447b6fa83a1c5d99ae0d89ec617fcb10b542 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 7 Sep 2017 21:55:24 +0100 Subject: [PATCH 054/138] Remove gyro overflow protection on MPU6500 and MPU9250 --- src/main/sensors/gyro.c | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0480a88925..4b717ef25e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -70,7 +70,7 @@ #include "hardware_revision.h" #endif -#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689))) +#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689))) #define USE_GYRO_SLEW_LIMITER #endif @@ -446,8 +446,9 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch #if defined(USE_GYRO_SLEW_LIMITER) void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) - gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0; + } } #endif @@ -499,7 +500,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { #if defined(USE_GYRO_SLEW_LIMITER) - gyroInitSlewLimiter(gyroSensor); + gyroInitSlewLimiter(gyroSensor); #endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); @@ -600,20 +601,20 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t } #if defined(USE_GYRO_SLEW_LIMITER) -int32_t gyroSlewLimiter(int axis, gyroSensor_t *gyroSensor) +int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) { - int32_t newRawGyro = (int32_t) gyroSensor->gyroDev.gyroADCRaw[axis]; + int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis]; - if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) - newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; - else - gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { + newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; + } else { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + } - return newRawGyro; + return newRawGyro; } #endif - void gyroUpdateSensor(gyroSensor_t *gyroSensor) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -622,14 +623,14 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { - // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations + // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations - gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; - gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; + gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; + gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; #if defined(USE_GYRO_SLEW_LIMITER) - gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(Z, gyroSensor) - gyroSensor->gyroDev.gyroZero[Z]; + gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z]; #else - gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; + gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; #endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); From b382c01cc0da30ccb34cba72bb9c0cb103ad914b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 7 Sep 2017 21:43:40 +0100 Subject: [PATCH 055/138] Better check for compression method in fc_msp --- src/main/fc/fc_msp.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9896808b96..97ebabec0a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -310,7 +310,12 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin sbufWriteU32(dst, address); // legacy format does not support compression +#ifdef USE_HUFFMAN const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN; +#else + const uint8_t compressionMethod = NO_COMPRESSION; + UNUSED(allowCompression); +#endif if (compressionMethod == NO_COMPRESSION) { if (!useLegacyFormat) { From cbaad53c462fcfe20e83e792d8e8b7c264fe79ee Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 8 Sep 2017 00:14:30 +0100 Subject: [PATCH 056/138] Do runtime check for gyro slew filter --- src/main/sensors/gyro.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4b717ef25e..94e226260f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -76,6 +76,7 @@ gyro_t gyro; static uint8_t gyroDebugMode; +static bool gyroUseSlewFilter; typedef struct gyroCalibration_s { @@ -363,6 +364,18 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) break; } + switch (gyroHardware) { + case GYRO_ICM20601: + case GYRO_ICM20602: + case GYRO_ICM20608G: + case GYRO_ICM20689: + gyroUseSlewFilter = true; + break; + default: + gyroUseSlewFilter = false; + break; + } + // Must set gyro targetLooptime before gyroDev.init and initialisation of filters gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf; @@ -604,6 +617,9 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) { int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis]; + if (gyroUseSlewFilter == false) { + return newRawGyro; + } if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; From b1f26666615cac0d3b4483467ca86b14809a6533 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Thu, 7 Sep 2017 21:12:46 -0400 Subject: [PATCH 057/138] Restored VTX and CMS defines --- src/main/target/ALIENWHOOP/target.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/ALIENWHOOP/target.mk b/src/main/target/ALIENWHOOP/target.mk index 39fe6f76ae..f1f9cd8a7c 100644 --- a/src/main/target/ALIENWHOOP/target.mk +++ b/src/main/target/ALIENWHOOP/target.mk @@ -22,4 +22,5 @@ TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_ak8963.c \ - drivers/max7456.c + drivers/max7456.c \ + io/osd.c From ba2567122c58579b3510d52a668ecec65e677395 Mon Sep 17 00:00:00 2001 From: "brucesdad13@gmail.com" Date: Thu, 7 Sep 2017 21:13:09 -0400 Subject: [PATCH 058/138] Restored VTX and CMS defines --- src/main/target/ALIENWHOOP/target.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/target/ALIENWHOOP/target.h b/src/main/target/ALIENWHOOP/target.h index 921ed5e538..0777d9fab2 100644 --- a/src/main/target/ALIENWHOOP/target.h +++ b/src/main/target/ALIENWHOOP/target.h @@ -201,10 +201,12 @@ #define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP ) // TODO FEATURE_OSD for V3 board ... FEATURE_TELEMETRY changes bind pin from rx to tx #endif +/* OSD currently dependent upon CMS, SMARTAUDIO, TRAMP #undef VTX_COMMON #undef VTX_CONTROL #undef VTX_SMARTAUDIO #undef VTX_TRAMP +*/ /* OLED Support */ @@ -217,7 +219,7 @@ #define I2C1_SCL PB6 #define I2C1_SDA PB7 #else -#undef CMS +//#undef CMS // TODO: OSD depends upon CMS #undef USE_I2C #endif From 8c0dd6bc065a54ec68827b35561ca7ef8264bf98 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 8 Sep 2017 11:02:27 +0900 Subject: [PATCH 059/138] Configure inverter for UART3 --- src/main/target/OMNIBUSF4/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index dc68273884..f2bbd61996 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -43,7 +43,10 @@ #define BEEPER_INVERTED #ifdef OMNIBUSF4SD +// These inverter control pins collide with timer channels on CH5 and CH6 pads. +// Users of these timers/pads must un-map the inverter assignment explicitly. #define INVERTER_PIN_UART6 PC8 // Omnibus F4 V3 and later +#define INVERTER_PIN_UART3 PC9 // Omnibus F4 Pro Corners #else #define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO XXX this is not used --- remove it at the next major release #endif From f5d2f534b4a9a940d2fe6dadaa3136b099fe9f1f Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 8 Sep 2017 02:19:22 +1200 Subject: [PATCH 060/138] Fixed setting of notch filter cutoffs to 0 in CLI. --- src/main/fc/settings.c | 6 +++--- src/main/flight/pid.c | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8ff270544c..d457fe76d0 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -308,9 +308,9 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) }, { "gyro_lowpass_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) }, - { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, - { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, + { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, #if defined(GYRO_USES_SPI) #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) @@ -556,7 +556,7 @@ const clivalue_t valueTable[] = { { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz) }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) }, - { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, + { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fdede37e33..670b8213f9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -172,7 +172,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) const uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed - float dTermNotchHz; + uint16_t dTermNotchHz; if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) { dTermNotchHz = pidProfile->dterm_notch_hz; } else { @@ -183,8 +183,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } - static biquadFilter_t biquadFilterNotch[2]; - if (dTermNotchHz) { + if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) { + static biquadFilter_t biquadFilterNotch[2]; dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff); for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { From 81c1153533e75c90d8260d11b403cee50a23ed6d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 4 Sep 2017 09:01:31 +0100 Subject: [PATCH 061/138] Added some sbuf utility functions --- src/main/common/crc.c | 22 ++++++++++++++++++++++ src/main/common/crc.h | 5 ++++- src/main/common/streambuf.c | 6 ++++++ src/main/common/streambuf.h | 5 +++-- 4 files changed, 35 insertions(+), 3 deletions(-) diff --git a/src/main/common/crc.c b/src/main/common/crc.c index 1ccbd78e5e..afd77b79d1 100644 --- a/src/main/common/crc.c +++ b/src/main/common/crc.c @@ -77,3 +77,25 @@ void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start) } sbufWriteU8(dst, crc); } + +uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length) +{ + const uint8_t *p = (const uint8_t *)data; + const uint8_t *pend = p + length; + + for (; p != pend; p++) { + crc ^= *p; + } + return crc; +} + +void crc8_xor_sbuf_append(sbuf_t *dst, uint8_t *start) +{ + uint8_t crc = 0; + const uint8_t *end = dst->ptr; + for (uint8_t *ptr = start; ptr < end; ++ptr) { + crc ^= *ptr; + } + sbufWriteU8(dst, crc); +} + diff --git a/src/main/common/crc.h b/src/main/common/crc.h index 8e8fbf5557..8aebc3c415 100644 --- a/src/main/common/crc.h +++ b/src/main/common/crc.h @@ -17,9 +17,12 @@ #pragma once +struct sbuf_s; + uint16_t crc16_ccitt(uint16_t crc, unsigned char a); uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length); uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a); uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length); -struct sbuf_s; void crc8_dvb_s2_sbuf_append(struct sbuf_s *dst, uint8_t *start); +uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length); +void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start); diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index 976439ae81..ad7db189d3 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -61,6 +61,12 @@ void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val) } +void sbufFill(sbuf_t *dst, uint8_t data, int len) +{ + memset(dst->ptr, data, len); + dst->ptr += len; +} + void sbufWriteData(sbuf_t *dst, const void *data, int len) { memcpy(dst->ptr, data, len); diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index 790423350e..21f5822fb6 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -20,19 +20,20 @@ #include // simple buffer-based serializer/deserializer without implicit size check -// little-endian encoding implemneted now typedef struct sbuf_s { - uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *ptr; // data pointer must be first (sbuf_t* is equivalent to uint8_t **) uint8_t *end; } sbuf_t; sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end); + void sbufWriteU8(sbuf_t *dst, uint8_t val); void sbufWriteU16(sbuf_t *dst, uint16_t val); void sbufWriteU32(sbuf_t *dst, uint32_t val); void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val); void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val); +void sbufFill(sbuf_t *dst, uint8_t data, int len); void sbufWriteData(sbuf_t *dst, const void *data, int len); void sbufWriteString(sbuf_t *dst, const char *string); void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string); From fb04fb1216d3474d516de3d3c927874a21f49737 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 8 Sep 2017 13:13:34 +0100 Subject: [PATCH 062/138] Remove gyro runtime slew filter check --- src/main/sensors/gyro.c | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 94e226260f..49bf26ab00 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -76,7 +76,6 @@ gyro_t gyro; static uint8_t gyroDebugMode; -static bool gyroUseSlewFilter; typedef struct gyroCalibration_s { @@ -364,18 +363,6 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) break; } - switch (gyroHardware) { - case GYRO_ICM20601: - case GYRO_ICM20602: - case GYRO_ICM20608G: - case GYRO_ICM20689: - gyroUseSlewFilter = true; - break; - default: - gyroUseSlewFilter = false; - break; - } - // Must set gyro targetLooptime before gyroDev.init and initialisation of filters gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf; @@ -617,16 +604,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) { int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis]; - if (gyroUseSlewFilter == false) { - return newRawGyro; - } - if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; } else { gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; } - return newRawGyro; } #endif From 00414b29b6a964505703f75b1bfe8a8680abf90e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 8 Sep 2017 14:58:47 +0200 Subject: [PATCH 063/138] Correct setpoint behaviour --- src/main/config/config_eeprom.h | 2 +- src/main/flight/pid.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 408a8e11aa..382e7d14c2 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -20,7 +20,7 @@ #include #include -#define EEPROM_CONF_VERSION 162 +#define EEPROM_CONF_VERSION 163 bool isEEPROMContentValid(void); bool loadEEPROM(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 670b8213f9..fdd7615f69 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -99,7 +99,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, .setpointRelaxRatio = 100, - .dtermSetpointWeight = 60, + .dtermSetpointWeight = 0, .yawRateAccelLimit = 100, .rateAccelLimit = 0, .itermThrottleThreshold = 350, @@ -488,7 +488,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered); float dynC = 0; - if ( (pidProfile->setpointRelaxRatio < 100) && (!flightModeFlags) ) { + if ( (pidProfile->dtermSetpointWeight > 0) && (!flightModeFlags) ) { dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y From 1769471da88965a3ff1c215156ec6bd94dc784a6 Mon Sep 17 00:00:00 2001 From: Spencer Owen Date: Fri, 8 Sep 2017 09:35:59 -0600 Subject: [PATCH 064/138] Adds vagrant file to gitignore file --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 63a87cbe41..7996fc5e51 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,6 @@ stm32.mak # artefacts for CLion /cmake-build-debug/ /CMakeLists.txt + +.vagrant +ubuntu*.log From 8562b7e60643b5af33e7ed5245cc8b9099cbe57c Mon Sep 17 00:00:00 2001 From: Spencer Owen Date: Fri, 8 Sep 2017 10:06:19 -0600 Subject: [PATCH 065/138] Sync time inside vagrant Resolves issue #4099 --- Vagrantfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Vagrantfile b/Vagrantfile index ad8698227a..cf7ec4a5a2 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -26,7 +26,8 @@ Vagrant.configure(2) do |config| add-apt-repository ppa:team-gcc-arm-embedded/ppa apt-get update apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1 - apt-get install -y make python gcc clang + apt-get install -y make python gcc clang ntpdate + ntpdate -u pool.ntp.org SHELL end From f4aed740c1d5e111a8c42d6d5b04181fee21b77c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 8 Sep 2017 18:31:01 +0100 Subject: [PATCH 066/138] Reset default debug_mode to none --- src/main/target/common_fc_pre.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index a6828777f9..84260118f9 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -26,7 +26,7 @@ //#pragma GCC diagnostic warning "-Wpadded" //#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons -#define DEBUG_MODE DEBUG_GYRO_NOTCH // change this to change initial debug mode +#define DEBUG_MODE DEBUG_NONE // change this to change initial debug mode #define I2C1_OVERCLOCK true #define I2C2_OVERCLOCK true From c1a99817f42172794c576efd402641a628c99272 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 9 Sep 2017 07:03:15 +0100 Subject: [PATCH 067/138] Minor tidy of PID code --- src/main/cms/cms_menu_imu.c | 2 +- src/main/common/filter.c | 9 ++++++--- src/main/common/filter.h | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/flight/pid.c | 25 +++++++++++++------------ src/main/flight/pid.h | 3 +-- 6 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 8107c1046b..f364cbca12 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -420,7 +420,7 @@ static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr) UNUSED(ptr); if (cmsx_dstPidProfile > 0) { - copyPidProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex()); + pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex()); } return 0; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 35bc54a794..c9585e857c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -295,6 +295,7 @@ float firFilterLastInput(const firFilter_t *filter) void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { + memset(filter, 0, sizeof(firFilterDenoise_t)); filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE); } @@ -303,12 +304,14 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input) { filter->state[filter->index] = input; filter->movingSum += filter->state[filter->index++]; - if (filter->index == filter->targetCount) + if (filter->index == filter->targetCount) { filter->index = 0; + } filter->movingSum -= filter->state[filter->index]; - if (filter->targetCount >= filter->filledCount) + if (filter->targetCount >= filter->filledCount) { return filter->movingSum / filter->targetCount; - else + } else { return filter->movingSum / ++filter->filledCount + 1; + } } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 730298cdbf..5b282a91e2 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -44,7 +44,7 @@ typedef struct biquadFilter_s { float x1, x2, y1, y2; } biquadFilter_t; -typedef struct firFilterDenoise_s{ +typedef struct firFilterDenoise_s { int filledCount; int targetCount; int index; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 97ebabec0a..aae0ec773c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1326,7 +1326,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) uint8_t dstProfileIndex = sbufReadU8(src); uint8_t srcProfileIndex = sbufReadU8(src); if (value == 0) { - copyPidProfile(dstProfileIndex, srcProfileIndex); + pidCopyProfile(dstProfileIndex, srcProfileIndex); } else if (value == 1) { copyControlRateProfile(dstProfileIndex, srcProfileIndex); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fdd7615f69..c8687ba162 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -126,7 +126,7 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) } } -void pidSetTargetLooptime(uint32_t pidLooptime) +static void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; dT = (float)targetPidLooptime * 0.000001f; @@ -199,8 +199,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) { dtermLpfApplyFn = nullFilterApply; } else { - memset(&dtermFilterLpfUnion, 0, sizeof(dtermFilterLpfUnion)); - switch (pidProfile->dterm_filter_type) { default: dtermLpfApplyFn = nullFilterApply; @@ -294,6 +292,16 @@ void pidInit(const pidProfile_t *pidProfile) pidInitMixer(pidProfile); } + +void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex) +{ + if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1) + && dstPidProfileIndex != srcPidProfileIndex + ) { + memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t)); + } +} + // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling static float calcHorizonLevelStrength(void) { @@ -371,7 +379,8 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit return currentPidSetpoint; } -static float accelerationLimit(int axis, float currentPidSetpoint) { +static float accelerationLimit(int axis, float currentPidSetpoint) +{ static float previousSetpoint[3]; const float currentVelocity = currentPidSetpoint- previousSetpoint[axis]; @@ -523,11 +532,3 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } } - -void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex) { - if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1) - && dstPidProfileIndex != srcPidProfileIndex - ) { - memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t)); - } -} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 98a5459ac5..c48a5eecff 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,11 +128,10 @@ extern uint8_t PIDweight[3]; void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void pidSetTargetLooptime(uint32_t pidLooptime); void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile); -void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex); +void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex); #endif From 5b41fed90f7a77523a74914ef510db5951716ef4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 9 Sep 2017 16:50:42 +0100 Subject: [PATCH 068/138] Reserve MSP value for setting clock --- src/main/msp/msp_protocol.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 21bc7d3faf..fbcc14ee4f 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -318,3 +318,4 @@ #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration #define MSP_SET_4WAY_IF 245 //in message Sets 4way interface +#define MSP_SET_RTC 246 //in message Sets the RTC clock From 8c4b898128351f54662732fbc543afcbf192bf08 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Sat, 9 Sep 2017 23:17:45 +0300 Subject: [PATCH 069/138] Added internal resistance adjustment --- src/main/drivers/camera_control.c | 4 ++-- src/main/drivers/camera_control.h | 3 +++ src/main/fc/settings.c | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c index a3abca17ed..e1daf12994 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/camera_control.c @@ -63,6 +63,7 @@ PG_RESET_TEMPLATE(cameraControlConfig_t, cameraControlConfig, .mode = CAMERA_CONTROL_MODE_HARDWARE_PWM, .refVoltage = 330, .keyDelayMs = 180, + .internalResistance = 470, .ioTag = IO_TAG(CAMERA_CONTROL_PIN) ); @@ -158,13 +159,12 @@ void cameraControlProcess(uint32_t currentTimeUs) } } -static const int cameraPullUpResistance = 47000; static const int buttonResistanceValues[] = { 45000, 27000, 15000, 6810, 0 }; static float calculateKeyPressVoltage(const cameraControlKey_e key) { const int buttonResistance = buttonResistanceValues[key]; - return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (cameraPullUpResistance + buttonResistance); + return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance); } #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE) diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h index 674aa9905e..cafc58b955 100644 --- a/src/main/drivers/camera_control.h +++ b/src/main/drivers/camera_control.h @@ -38,8 +38,11 @@ typedef enum { typedef struct cameraControlConfig_s { cameraControlMode_e mode; + // measured in 10 mV steps uint16_t refVoltage; uint16_t keyDelayMs; + // measured 100 Ohm steps + uint16_t internalResistance; ioTag_t ioTag; } cameraControlConfig_t; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index d457fe76d0..280319f8a3 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -777,6 +777,7 @@ const clivalue_t valueTable[] = { { "camera_control_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CAMERA_CONTROL_MODE }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, mode) }, { "camera_control_ref_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 200, 400 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, refVoltage) }, { "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) }, + { "camera_control_internal_resistance", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 1000 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, internalResistance) }, #endif }; From 1594c6b19c2e389c1d08b92025ce827b749caec0 Mon Sep 17 00:00:00 2001 From: Spencer Owen Date: Sat, 9 Sep 2017 16:03:39 -0600 Subject: [PATCH 070/138] Use timedatectl instead of ntp --- Vagrantfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index cf7ec4a5a2..044a1be33e 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -26,8 +26,8 @@ Vagrant.configure(2) do |config| add-apt-repository ppa:team-gcc-arm-embedded/ppa apt-get update apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1 - apt-get install -y make python gcc clang ntpdate - ntpdate -u pool.ntp.org + apt-get install -y make python gcc clang ntp + timedatectl set-ntp true SHELL end From d4b1c06bf8fc378dfdc4dfb31bf45412e862458d Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 10 Sep 2017 12:55:47 +1200 Subject: [PATCH 071/138] Added delay before sending Dshot commands to avoid read errors. --- src/main/fc/cli.c | 47 ++++++++++++++++++++++++++----------------- src/main/fc/fc_core.c | 1 + src/main/io/beeper.c | 5 +++-- 3 files changed, 32 insertions(+), 21 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b1a5c742e0..d969a61685 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2456,6 +2456,7 @@ static void cliDshotProg(char *cmdline) char *pch = strtok_r(cmdline, " ", &saveptr); int pos = 0; int escIndex = 0; + bool firstCommand = true; while (pch != NULL) { switch (pos) { case 0: @@ -2466,33 +2467,41 @@ static void cliDshotProg(char *cmdline) break; default: - pwmDisableMotors(); + { + int command = atoi(pch); + if (command >= 0 && command < DSHOT_MIN_THROTTLE) { + if (firstCommand) { + pwmDisableMotors(); - int command = atoi(pch); - if (command >= 0 && command < DSHOT_MIN_THROTTLE) { - if (command == DSHOT_CMD_ESC_INFO) { - delay(5); // Wait for potential ESC telemetry transmission to finish - } + if (command == DSHOT_CMD_ESC_INFO) { + delay(5); // Wait for potential ESC telemetry transmission to finish + } else { + delay(1); + } - if (command != DSHOT_CMD_ESC_INFO) { - pwmWriteDshotCommand(escIndex, getMotorCount(), command); - } else { - if (escIndex != ALL_MOTORS) { - executeEscInfoCommand(escIndex); + firstCommand = false; + } + + if (command != DSHOT_CMD_ESC_INFO) { + pwmWriteDshotCommand(escIndex, getMotorCount(), command); } else { - for (uint8_t i = 0; i < getMotorCount(); i++) { - executeEscInfoCommand(i); + if (escIndex != ALL_MOTORS) { + executeEscInfoCommand(escIndex); + } else { + for (uint8_t i = 0; i < getMotorCount(); i++) { + executeEscInfoCommand(i); + } } } - } - cliPrintLinef("Command %d written.", command); + cliPrintLinef("Command %d written.", command); - if (command <= 5) { - delay(20); // wait for sound output to finish + if (command <= 5) { + delay(20); // wait for sound output to finish + } + } else { + cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1); } - } else { - cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1); } break; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 6a37dadaae..22c3ea0084 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -265,6 +265,7 @@ void tryArm(void) #ifdef USE_DSHOT if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { pwmDisableMotors(); + delay(1); if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { flipOverAfterCrashMode = false; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index a51cb925cd..4f803b430a 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -364,11 +364,12 @@ void beeperUpdate(timeUs_t currentTimeUs) #ifdef USE_DSHOT if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { - pwmDisableMotors(); + pwmDisableMotors(); + delay(1); pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3); - pwmEnableMotors(); + pwmEnableMotors(); } #endif From c9e8bae51d603f50f05e9c669707426542310453 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 10 Sep 2017 13:55:56 +1200 Subject: [PATCH 072/138] Fixed beeping for 'gyro_cal_on_first_arm'. --- src/main/sensors/gyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 49bf26ab00..ccbf38abd1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -592,7 +592,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics - if (!firstArmingCalibrationWasStarted || !isArmingDisabled()) { + if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) { beeper(BEEPER_GYRO_CALIBRATED); } } From 863b24ae6a061d760355527d40a1f9450bca9a4d Mon Sep 17 00:00:00 2001 From: Curtis Bangert Date: Fri, 25 Aug 2017 09:22:02 -0400 Subject: [PATCH 073/138] Corrected smartport regression issue with msp_shared Corrected condition where an extra padded zero was added to outgoing msp buffers. Whitespace tidy and removal of unnecessary condition. Commented OSD and CMS from STM32F3DISCOVERY target.h Various improvements to msp_shared (removed byte loops, point arithmetic) Raised schedule priority of CRSF device and MSP calls Reworked buffers in msp_shared to enable writes. Moved msp buffers to msp_shared Added new custom frames and reworking msp implementation Implemented CRSF device info reply Adding crsf ping/pong device info functionality Changed Colibri FC address to Betaflight address Implementing MSP in CRSF Telemetry Decoupled msp functionality from smartport into msp_shared Moved USE_SERVOS to FLASH>128 per mikeller --- make/source.mk | 1 + src/main/rx/crsf.c | 28 +- src/main/rx/crsf.h | 19 +- src/main/target/SPRACINGF3EVO/target.h | 2 + src/main/target/STM32F3DISCOVERY/target.h | 4 +- src/main/target/common_fc_pre.h | 2 +- src/main/telemetry/crsf.c | 79 ++++- src/main/telemetry/crsf.h | 3 + src/main/telemetry/msp_shared.c | 231 ++++++++++++++ src/main/telemetry/msp_shared.h | 29 ++ src/main/telemetry/smartport.c | 225 +------------- src/main/telemetry/smartport.h | 3 + src/main/telemetry/telemetry.c | 2 + src/test/Makefile | 24 +- src/test/unit/rx_crsf_unittest.cc | 7 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 299 +++++++++++++++++++ src/test/unit/telemetry_crsf_unittest.cc | 9 + 17 files changed, 738 insertions(+), 229 deletions(-) create mode 100644 src/main/telemetry/msp_shared.c create mode 100644 src/main/telemetry/msp_shared.h create mode 100644 src/test/unit/telemetry_crsf_msp_unittest.cc diff --git a/make/source.mk b/make/source.mk index c668bbc113..bc6c3e4913 100644 --- a/make/source.mk +++ b/make/source.mk @@ -159,6 +159,7 @@ FC_SRC = \ telemetry/smartport.c \ telemetry/ltm.c \ telemetry/mavlink.c \ + telemetry/msp_shared.c \ telemetry/ibus.c \ telemetry/ibus_shared.c \ sensors/esc_sensor.c \ diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 7ef216f3f4..f37cba0ae4 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -33,22 +33,29 @@ #include "drivers/serial.h" #include "drivers/serial_uart.h" +#include "drivers/system.h" #include "drivers/time.h" #include "io/serial.h" +#include "msp/msp.h" + #include "rx/rx.h" #include "rx/crsf.h" +#include "telemetry/crsf.h" +#include "telemetry/msp_shared.h" + #define CRSF_TIME_NEEDED_PER_FRAME_US 1000 #define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 +#define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) + STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; - STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; @@ -56,7 +63,6 @@ static uint32_t crsfFrameStartAt = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; - /* * CRSF protocol * @@ -133,6 +139,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) if (crsfFramePosition < fullFrameLength) { crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; + if (crsfFrameDone) { + crsfFramePosition = 0; + } } } @@ -176,6 +185,20 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) crsfChannelData[14] = rcChannels->chan14; crsfChannelData[15] = rcChannels->chan15; return RX_FRAME_COMPLETE; + } else { + if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) { + // TODO: CRC CHECK + scheduleDeviceInfoResponse(); + return RX_FRAME_COMPLETE; + } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) { + // TODO: CRC CHECK + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + if(handleMspFrame(frameStart, frameEnd)) { + scheduleMspResponse(); + } + return RX_FRAME_COMPLETE; + } } } return RX_FRAME_PENDING; @@ -222,6 +245,7 @@ void crsfRxSendTelemetryData(void) bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { + for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408; } diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 383ac0ac74..0b547cbc67 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -22,6 +22,8 @@ #define CRSF_PORT_MODE MODE_RXTX #define CRSF_MAX_CHANNEL 16 +#define CRSF_MSP_RX_BUF_SIZE 128 +#define CRSF_MSP_TX_BUF_SIZE 128 typedef enum { CRSF_FRAMETYPE_GPS = 0x02, @@ -29,7 +31,12 @@ typedef enum { CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, - CRSF_FRAMETYPE_FLIGHT_MODE = 0x21 + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_FRAMETYPE_MSP_WRITE = 0x7C // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) } crsfFrameType_e; enum { @@ -38,11 +45,14 @@ enum { CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, + CRSF_FRAME_TX_MSP_PAYLOAD_SIZE = 58, + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE = 8, CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field - CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4 // length of Extended Dest/Origin, TYPE and CRC fields combined }; enum { @@ -51,7 +61,7 @@ enum { CRSF_ADDRESS_RESERVED1 = 0x8A, CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, - CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8, + CRSF_ADDRESS_BETAFLIGHT = 0xC8, CRSF_ADDRESS_RESERVED2 = 0xCA, CRSF_ADDRESS_RACE_TAG = 0xCC, CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, @@ -59,7 +69,7 @@ enum { CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE }; -#define CRSF_PAYLOAD_SIZE_MAX 32 // !!TODO needs checking +#define CRSF_PAYLOAD_SIZE_MAX 60 // Size confirmed by Remo #define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) typedef struct crsfFrameDef_s { @@ -74,7 +84,6 @@ typedef union crsfFrame_u { crsfFrameDef_t frame; } crsfFrame_t; - void crsfRxWriteTelemetryData(const void *data, int len); void crsfRxSendTelemetryData(void); diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index ffc531b982..7916af9ef3 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -188,3 +188,5 @@ #else #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16)) #endif + +#undef USE_DASHBOARD diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 0ff719e5d0..cec8f5e4a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -129,12 +129,12 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 -#define OSD +//#define OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN SPI2_NSS_PIN -#define CMS +//#define CMS //#define USE_SDCARD // diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 84260118f9..4f0c4165d5 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -102,7 +102,6 @@ #define TELEMETRY_LTM #define TELEMETRY_SMARTPORT #define USE_RESOURCE_MGMT -#define USE_SERVOS #endif #if (FLASH_SIZE > 128) @@ -118,6 +117,7 @@ #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS #define USE_SENSOR_NAMES +#define USE_SERVOS #define USE_VIRTUAL_CURRENT_METER #define VTX_COMMON #define VTX_CONTROL diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 79d2357cbc..c1deb636b3 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -24,6 +24,7 @@ #ifdef TELEMETRY #include "config/feature.h" +#include "build/build_config.h" #include "build/version.h" #include "config/parameter_group.h" @@ -31,6 +32,7 @@ #include "common/crc.h" #include "common/maths.h" +#include "common/printf.h" #include "common/streambuf.h" #include "common/utils.h" @@ -51,12 +53,17 @@ #include "telemetry/telemetry.h" #include "telemetry/crsf.h" +#include "telemetry/msp_shared.h" #include "fc/config.h" #define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz +#define CRSF_DEVICEINFO_VERSION 0x01 +#define CRSF_DEVICEINFO_PARAMETER_COUNT 255 static bool crsfTelemetryEnabled; +static bool deviceInfoReplyPending; +static bool mspReplyPending; static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; static void crsfInitializeFrame(sbuf_t *dst) @@ -223,6 +230,41 @@ void crsfFrameFlightMode(sbuf_t *dst) *lengthPtr = sbufPtr(dst) - lengthPtr; } +void scheduleDeviceInfoResponse() { + deviceInfoReplyPending = true; +} + +/* +0x29 Device Info +Payload: +uint8_t Destination +uint8_t Origin +char[] Device Name ( Null terminated string ) +uint32_t Null Bytes +uint32_t Null Bytes +uint32_t Null Bytes +uint8_t 255 (Max MSP Parameter) +uint8_t 0x01 (Parameter version 1) +*/ +void crsfFrameDeviceInfo(sbuf_t *dst) { + + char buff[30]; + tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier); + + uint8_t *lengthPtr = sbufPtr(dst); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO); + sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); + sbufWriteStringWithZeroTerminator(dst, buff); + for (unsigned int ii=0; ii<12; ii++) { + sbufWriteU8(dst, 0x00); + } + sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT); + sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION); + *lengthPtr = sbufPtr(dst) - lengthPtr; +} + #define BV(x) (1 << (x)) // bit value // schedule array to decide how often each type of frame is sent @@ -238,6 +280,25 @@ typedef enum { static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +void scheduleMspResponse() { + if (!mspReplyPending) { + mspReplyPending = true; + } +} + +void crsfSendMspResponse(uint8_t *payload) +{ + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + crsfInitializeFrame(dst); + sbufWriteU8(dst, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); + sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); + sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); + sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE); + crsfFinalize(dst); + } static void processCrsf(void) { @@ -257,6 +318,7 @@ static void processCrsf(void) crsfFrameBatterySensor(dst); crsfFinalize(dst); } + if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) { crsfInitializeFrame(dst); crsfFrameFlightMode(dst); @@ -277,6 +339,10 @@ void initCrsfTelemetry(void) // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); + + deviceInfoReplyPending = false; + mspReplyPending = false; + int index = 0; crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); @@ -311,7 +377,18 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) { crsfLastCycleTime = currentTimeUs; - processCrsf(); + if (deviceInfoReplyPending) { + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + crsfInitializeFrame(dst); + crsfFrameDeviceInfo(dst); + crsfFinalize(dst); + deviceInfoReplyPending = false; + } else if (mspReplyPending) { + mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse); + } else { + processCrsf(); + } } } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index e5a3e4611e..b183d2927b 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -19,9 +19,12 @@ #include "common/time.h" #include "rx/crsf.h" +#include "telemetry/msp_shared.h" void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); +void scheduleDeviceInfoResponse(); +void scheduleMspResponse(); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c new file mode 100644 index 0000000000..699571f3d7 --- /dev/null +++ b/src/main/telemetry/msp_shared.c @@ -0,0 +1,231 @@ +#include +#include +#include +#include +#include + +#include "platform.h" + +#ifdef TELEMETRY + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "fc/fc_msp.h" + +#include "msp/msp.h" + +#include "rx/crsf.h" +#include "rx/msp.h" + +#include "telemetry/msp_shared.h" +#include "telemetry/smartport.h" + +#define TELEMETRY_MSP_VERSION 1 +#define TELEMETRY_MSP_VER_SHIFT 5 +#define TELEMETRY_MSP_VER_MASK (0x7 << TELEMETRY_MSP_VER_SHIFT) +#define TELEMETRY_MSP_ERROR_FLAG (1 << 5) +#define TELEMETRY_MSP_START_FLAG (1 << 4) +#define TELEMETRY_MSP_SEQ_MASK 0x0F +#define TELEMETRY_MSP_RES_ERROR (-10) + +enum { + TELEMETRY_MSP_VER_MISMATCH=0, + TELEMETRY_MSP_CRC_ERROR=1, + TELEMETRY_MSP_ERROR=2 +}; + +#define REQUEST_BUFFER_SIZE 64 +#define RESPONSE_BUFFER_SIZE 64 + +STATIC_UNIT_TESTED uint8_t checksum = 0; +STATIC_UNIT_TESTED mspPackage_t mspPackage; +static mspRxBuffer_t mspRxBuffer; +static mspTxBuffer_t mspTxBuffer; +static mspPacket_t mspRxPacket; +static mspPacket_t mspTxPacket; + +void initSharedMsp() { + mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; + mspPackage.requestPacket = &mspRxPacket; + mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer; + mspPackage.requestPacket->buf.end = mspPackage.requestBuffer; + + mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer; + mspPackage.responsePacket = &mspTxPacket; + mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; +} + +static void processMspPacket() +{ + mspPackage.responsePacket->cmd = 0; + mspPackage.responsePacket->result = 0; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + + mspPostProcessFnPtr mspPostProcessFn = NULL; + if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { + sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); + } + if (mspPostProcessFn) { + mspPostProcessFn(NULL); + } + + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); +} + +void sendMspErrorResponse(uint8_t error, int16_t cmd) +{ + mspPackage.responsePacket->cmd = cmd; + mspPackage.responsePacket->result = 0; + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + + sbufWriteU8(&mspPackage.responsePacket->buf, error); + mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR; + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); +} + +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) +{ + static uint8_t mspStarted = 0; + static uint8_t lastSeq = 0; + + if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { + return false; + } + + if (mspStarted == 0) { + initSharedMsp(); + } + + mspPacket_t *packet = mspPackage.requestPacket; + sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd); + sbuf_t *rxBuf = &mspPackage.requestPacket->buf; + uint8_t header = sbufReadU8(frameBuf); + uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; + uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; + + if (version != TELEMETRY_MSP_VERSION) { + sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); + return true; + } + + if (header & TELEMETRY_MSP_START_FLAG) { + // first packet in sequence + uint8_t mspPayloadSize = sbufReadU8(frameBuf); + + packet->cmd = sbufReadU8(frameBuf); + packet->result = 0; + packet->buf.ptr = mspPackage.requestBuffer; + packet->buf.end = mspPackage.requestBuffer + mspPayloadSize; + + checksum = mspPayloadSize ^ packet->cmd; + mspStarted = 1; + } else if (!mspStarted) { + // no start packet yet, throw this one away + return false; + } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) { + // packet loss detected! + mspStarted = 0; + return false; + } + + uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); + uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); + uint8_t payload[frameBytesRemaining]; + + if (bufferBytesRemaining >= frameBytesRemaining) { + sbufReadData(frameBuf, payload, frameBytesRemaining); + sbufAdvance(frameBuf, frameBytesRemaining); + sbufWriteData(rxBuf, payload, frameBytesRemaining); + lastSeq = seqNumber; + return false; + } else { + sbufReadData(frameBuf, payload, bufferBytesRemaining); + sbufAdvance(frameBuf, bufferBytesRemaining); + sbufWriteData(rxBuf, payload, bufferBytesRemaining); + sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); + while (sbufBytesRemaining(rxBuf)) { + checksum ^= sbufReadU8(rxBuf); + } + + if (checksum != *frameBuf->ptr) { + mspStarted = 0; + sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd); + return true; + } + } + + mspStarted = 0; + sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); + processMspPacket(); + return true; +} + +bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) +{ + static uint8_t checksum = 0; + static uint8_t seq = 0; + + uint8_t payloadOut[payloadSize]; + sbuf_t payload; + sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize); + sbuf_t *txBuf = &mspPackage.responsePacket->buf; + + // detect first reply packet + if (txBuf->ptr == mspPackage.responseBuffer) { + + // header + uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK); + if (mspPackage.responsePacket->result < 0) { + head |= TELEMETRY_MSP_ERROR_FLAG; + } + sbufWriteU8(payloadBuf, head); + + uint8_t size = sbufBytesRemaining(txBuf); + sbufWriteU8(payloadBuf, size); + } + else { + // header + sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); + } + + uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); + uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); + uint8_t frame[payloadBytesRemaining]; + + if (bufferBytesRemaining >= payloadBytesRemaining) { + + sbufReadData(txBuf, frame, payloadBytesRemaining); + sbufAdvance(txBuf, payloadBytesRemaining); + sbufWriteData(payloadBuf, frame, payloadBytesRemaining); + responseFn(payloadOut); + + return true; + + } else { + + sbufReadData(txBuf, frame, bufferBytesRemaining); + sbufAdvance(txBuf, bufferBytesRemaining); + sbufWriteData(payloadBuf, frame, bufferBytesRemaining); + sbufSwitchToReader(txBuf, mspPackage.responseBuffer); + + checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd; + + while (sbufBytesRemaining(txBuf)) { + checksum ^= sbufReadU8(txBuf); + } + sbufWriteU8(payloadBuf, checksum); + + while (sbufBytesRemaining(payloadBuf)>1) { + sbufWriteU8(payloadBuf, 0); + } + + } + + responseFn(payloadOut); + return false; +} + +#endif diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h new file mode 100644 index 0000000000..315c47a615 --- /dev/null +++ b/src/main/telemetry/msp_shared.h @@ -0,0 +1,29 @@ +#pragma once + +#include "msp/msp.h" +#include "rx/crsf.h" +#include "telemetry/smartport.h" + +typedef void (*mspResponseFnPtr)(uint8_t *payload); + +typedef struct mspPackage_s { + sbuf_t requestFrame; + uint8_t *requestBuffer; + uint8_t *responseBuffer; + mspPacket_t *requestPacket; + mspPacket_t *responsePacket; +} mspPackage_t; + +typedef union mspRxBuffer_u { + uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; + uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE]; +} mspRxBuffer_t; + +typedef union mspTxBuffer_u { + uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE]; + uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; +} mspTxBuffer_t; + +void initSharedMsp(); +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); +bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 080df64911..e29e45fe2a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -43,8 +43,6 @@ #include "io/gps.h" #include "io/serial.h" -#include "msp/msp.h" - #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" @@ -54,10 +52,10 @@ #include "sensors/gyro.h" #include "rx/rx.h" -#include "rx/msp.h" #include "telemetry/telemetry.h" #include "telemetry/smartport.h" +#include "telemetry/msp_shared.h" enum { @@ -164,7 +162,6 @@ typedef struct smartPortFrame_s { } __attribute__((packed)) smartPortFrame_t; #define SMARTPORT_FRAME_SIZE sizeof(smartPortFrame_t) -#define SMARTPORT_TX_BUF_SIZE 256 #define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId) #define SMARTPORT_PAYLOAD_SIZE (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1) @@ -172,30 +169,8 @@ typedef struct smartPortFrame_s { static smartPortFrame_t smartPortRxBuffer; static uint8_t smartPortRxBytes = 0; static bool smartPortFrameReceived = false; - -#define SMARTPORT_MSP_VERSION 1 -#define SMARTPORT_MSP_VER_SHIFT 5 -#define SMARTPORT_MSP_VER_MASK (0x7 << SMARTPORT_MSP_VER_SHIFT) -#define SMARTPORT_MSP_VERSION_S (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT) - -#define SMARTPORT_MSP_ERROR_FLAG (1 << 5) -#define SMARTPORT_MSP_START_FLAG (1 << 4) -#define SMARTPORT_MSP_SEQ_MASK 0x0F - -#define SMARTPORT_MSP_RX_BUF_SIZE 64 - -static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE]; -static mspPacket_t smartPortMspReply; static bool smartPortMspReplyPending = false; -#define SMARTPORT_MSP_RES_ERROR (-10) - -enum { - SMARTPORT_MSP_VER_MISMATCH=0, - SMARTPORT_MSP_CRC_ERROR=1, - SMARTPORT_MSP_ERROR=2 -}; - static void smartPortDataReceive(uint16_t c) { static bool skipUntilStart = true; @@ -352,194 +327,8 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } -static void initSmartPortMspReply(int16_t cmd) -{ - smartPortMspReply.buf.ptr = smartPortMspTxBuffer; - smartPortMspReply.buf.end = ARRAYEND(smartPortMspTxBuffer); - - smartPortMspReply.cmd = cmd; - smartPortMspReply.result = 0; -} - -static void processMspPacket(mspPacket_t* packet) -{ - initSmartPortMspReply(0); - - if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR) { - sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR); - } - - // change streambuf direction - sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); - smartPortMspReplyPending = true; -} - -/** - * Request frame format: - * - Header: 1 byte - * - Reserved: 2 bits (future use) - * - Error-flag: 1 bit - * - Start-flag: 1 bit - * - CSeq: 4 bits - * - * - MSP payload: - * - if Error-flag == 0: - * - size: 1 byte - * - payload - * - CRC (request type included) - * - if Error-flag == 1: - * - size: 1 byte (== 1) - * - error: 1 Byte - * - 0: Version mismatch (type=0) - * - 1: Sequence number error - * - 2: MSP error - * - CRC (request type included) - */ -bool smartPortSendMspReply() -{ - static uint8_t checksum = 0; - static uint8_t seq = 0; - - uint8_t packet[SMARTPORT_PAYLOAD_SIZE]; - uint8_t* p = packet; - uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; - - sbuf_t* txBuf = &smartPortMspReply.buf; - - // detect first reply packet - if (txBuf->ptr == smartPortMspTxBuffer) { - - // header - uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK); - if (smartPortMspReply.result < 0) { - head |= SMARTPORT_MSP_ERROR_FLAG; - } - *p++ = head; - - uint8_t size = sbufBytesRemaining(txBuf); - *p++ = size; - - checksum = size ^ smartPortMspReply.cmd; - } - else { - // header - *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK); - } - - while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) { - *p = sbufReadU8(txBuf); - checksum ^= *p++; // MSP checksum - } - - // to be continued... - if (p == end) { - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return true; - } - - // nothing left in txBuf, - // append the MSP checksum - *p++ = checksum; - - // pad with zeros - while (p < end) - *p++ = 0; - - smartPortSendPackageEx(FSSP_MSPS_FRAME,packet); - return false; -} - -void smartPortSendErrorReply(uint8_t error, int16_t cmd) -{ - initSmartPortMspReply(cmd); - sbufWriteU8(&smartPortMspReply.buf,error); - smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR; - - sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer); - smartPortMspReplyPending = true; -} - -/** - * Request frame format: - * - Header: 1 byte - * - Version: 3 bits - * - Start-flag: 1 bit - * - CSeq: 4 bits - * - * - MSP payload: - * - Size: 1 Byte - * - Type: 1 Byte - * - payload... - * - CRC - */ -void handleSmartPortMspFrame(smartPortFrame_t* sp_frame) -{ - static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE]; - static uint8_t mspStarted = 0; - static uint8_t lastSeq = 0; - static uint8_t checksum = 0; - static mspPacket_t cmd; - - // re-assemble MSP frame & forward to MSP port when complete - uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET; - uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE; - - uint8_t head = *p++; - uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK; - uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT; - - if (version != SMARTPORT_MSP_VERSION) { - mspStarted = 0; - smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0); - return; - } - - // check start-flag - if (head & SMARTPORT_MSP_START_FLAG) { - - //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error! - uint8_t p_size = *p++; - cmd.cmd = *p++; - cmd.result = 0; - - cmd.buf.ptr = mspBuffer; - cmd.buf.end = mspBuffer + p_size; - - checksum = p_size ^ cmd.cmd; - mspStarted = 1; - } else if (!mspStarted) { - // no start packet yet, throw this one away - return; - } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) { - // packet loss detected! - mspStarted = 0; - return; - } - - // copy payload bytes - while ((p < end) && sbufBytesRemaining(&cmd.buf)) { - checksum ^= *p; - sbufWriteU8(&cmd.buf,*p++); - } - - // reached end of smart port frame - if (p == end) { - lastSeq = seq; - return; - } - - // last byte must be the checksum - if (checksum != *p) { - mspStarted = 0; - smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd); - return; - } - - // end of MSP packet reached - mspStarted = 0; - sbufSwitchToReader(&cmd.buf,mspBuffer); - - processMspPacket(&cmd); +void smartPortSendMspResponse(uint8_t *payload) { + smartPortSendPackageEx(FSSP_MSPS_FRAME, payload); } void handleSmartPortTelemetry(void) @@ -566,7 +355,11 @@ void handleSmartPortTelemetry(void) if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { // Pass only the payload: skip sensorId & frameId - handleSmartPortMspFrame(&smartPortRxBuffer); + if (!smartPortMspReplyPending) { + uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET; + uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE; + smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd); + } } } @@ -578,7 +371,7 @@ void handleSmartPortTelemetry(void) } if (smartPortMspReplyPending) { - smartPortMspReplyPending = smartPortSendMspReply(); + smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse); smartPortHasRequest = 0; return; } diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 35f18fc4ae..2c918cb249 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,6 +7,9 @@ #pragma once +#define SMARTPORT_MSP_TX_BUF_SIZE 256 +#define SMARTPORT_MSP_RX_BUF_SIZE 64 + void initSmartPortTelemetry(void); void handleSmartPortTelemetry(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 304271d8c9..9f9fe12663 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -52,6 +52,7 @@ #include "telemetry/crsf.h" #include "telemetry/srxl.h" #include "telemetry/ibus.h" +#include "telemetry/msp_shared.h" PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); @@ -100,6 +101,7 @@ void telemetryInit(void) #ifdef TELEMETRY_IBUS initIbusTelemetry(); #endif + initSharedMsp(); telemetryCheckState(); } diff --git a/src/test/Makefile b/src/test/Makefile index cb5b3a6d05..7a18803e3c 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -172,7 +172,10 @@ rc_controls_unittest_SRC := \ rx_crsf_unittest_SRC := \ $(USER_DIR)/rx/crsf.c \ $(USER_DIR)/common/crc.c \ - $(USER_DIR)/common/streambuf.c + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/drivers/serial.c rx_ibus_unittest_SRC := \ @@ -207,8 +210,24 @@ telemetry_crsf_unittest_SRC := \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/common/streambuf.c \ $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/typeconversion.c \ $(USER_DIR)/fc/runtime_config.c +telemetry_crsf_msp_unittest_SRC := \ + $(USER_DIR)/rx/crsf.c \ + $(USER_DIR)/common/crc.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/common/printf.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/drivers/serial.c \ + $(USER_DIR)/common/typeconversion.c \ + $(USER_DIR)/telemetry/crsf.c \ + $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/telemetry/msp_shared.c \ + $(USER_DIR)/fc/runtime_config.c + + telemetry_crsf_unittest_DEFINES := \ FLASH_SIZE=128 \ STM32F10X_MD \ @@ -254,6 +273,9 @@ huffman_unittest_SRC := \ huffman_unittest_DEFINES := \ USE_HUFFMAN +ringbuffer_unittest_SRC := \ + $(USER_DIR)/common/ringbuffer.c + # Please tweak the following variable definitions as needed by your # project, except GTEST_HEADERS, which you can use in your own targets # but shouldn't modify. diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index a98ed6c9e4..09fa725b5f 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -29,11 +29,14 @@ extern "C" { #include "common/crc.h" #include "common/utils.h" + #include "drivers/serial.h" #include "io/serial.h" #include "rx/rx.h" #include "rx/crsf.h" + #include "telemetry/msp_shared.h" + void crsfDataReceive(uint16_t c); uint8_t crsfFrameCRC(void); uint8_t crsfFrameStatus(void); @@ -277,7 +280,9 @@ int16_t debug[DEBUG16_VALUE_COUNT]; uint32_t micros(void) {return dummyTimeUs;} serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} -void serialWriteBuf(serialPort_t *, const uint8_t *, int) {} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; +void scheduleDeviceInfoResponse(void) {}; +void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; +bool handleMspFrame(uint8_t *, uint8_t *) { return false; } } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc new file mode 100644 index 0000000000..8ef8099531 --- /dev/null +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -0,0 +1,299 @@ +/* + * 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 +#include + +extern "C" { + #include + + #include "build/debug.h" + + #include "common/crc.h" + #include "common/utils.h" + #include "common/printf.h" + #include "common/gps_conversion.h" + #include "common/streambuf.h" + #include "common/typeconversion.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "drivers/serial.h" + #include "drivers/system.h" + + #include "fc/runtime_config.h" + #include "fc/config.h" + #include "flight/imu.h" + #include "fc/fc_msp.h" + + #include "io/serial.h" + #include "io/gps.h" + + #include "msp/msp.h" + + #include "rx/rx.h" + #include "rx/crsf.h" + + #include "sensors/battery.h" + #include "sensors/sensors.h" + + #include "telemetry/telemetry.h" + #include "telemetry/msp_shared.h" + #include "telemetry/smartport.h" + + bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); + bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); + uint8_t sbufReadU8(sbuf_t *src); + int sbufBytesRemaining(sbuf_t *buf); + void initSharedMsp(); + uint16_t testBatteryVoltage = 0; + int32_t testAmperage = 0; + uint8_t mspTxData[64]; //max frame size + sbuf_t mspTxDataBuf; + uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX]; + uint8_t payloadOutput[64]; + sbuf_t payloadOutputBuf; + int32_t testmAhDrawn = 0; + + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + + extern bool crsfFrameDone; + extern crsfFrame_t crsfFrame; + extern mspPackage_t mspPackage; + extern uint8_t checksum; + + uint32_t dummyTimeUs; + +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +typedef struct crsfMspFrame_s { + uint8_t deviceAddress; + uint8_t frameLength; + uint8_t type; + uint8_t destination; + uint8_t origin; + uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC]; +} crsfMspFrame_t; + +const uint8_t crsfPidRequest[] = { + 0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69 +}; + +TEST(CrossFireMSPTest, RequestBufferTest) +{ + initSharedMsp(); + const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest; + crsfFrame = *(const crsfFrame_t*)framePtr; + crsfFrameDone = true; + EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress); + EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE, crsfFrame.frame.frameLength); + EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type); + uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload; + uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + EXPECT_EQ(0xC8, *destination); + EXPECT_EQ(0xEA, *origin); + EXPECT_EQ(0x30, *frameStart); + EXPECT_EQ(0x69, *frameEnd); +} + +TEST(CrossFireMSPTest, ResponsePacketTest) +{ + initSharedMsp(); + const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest; + crsfFrame = *(const crsfFrame_t*)framePtr; + crsfFrameDone = true; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + handleMspFrame(frameStart, frameEnd); + for (unsigned int ii=1; ii<30; ii++) { + EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf)); + } + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); +} + +const uint8_t crsfPidWrite1[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32}; +const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00}; +const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19}; +const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00}; +const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00}; + +TEST(CrossFireMSPTest, WriteResponseTest) +{ + initSharedMsp(); + const crsfMspFrame_t *framePtr1 = (const crsfMspFrame_t*)crsfPidWrite1; + crsfFrame = *(const crsfFrame_t*)framePtr1; + crsfFrameDone = true; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + bool pending1 = handleMspFrame(frameStart, frameEnd); + EXPECT_FALSE(pending1); // not done yet*/ + EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); + EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); + EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]); + EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[4]); + + const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2; + crsfFrame = *(const crsfFrame_t*)framePtr2; + crsfFrameDone = true; + uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + bool pending2 = handleMspFrame(frameStart2, frameEnd2); + EXPECT_FALSE(pending2); // not done yet + EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); + EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); + EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]); + EXPECT_EQ(0x14, mspPackage.requestBuffer[8]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[9]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[10]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[11]); + + const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3; + crsfFrame = *(const crsfFrame_t*)framePtr3; + crsfFrameDone = true; + uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + bool pending3 = handleMspFrame(frameStart3, frameEnd3); + EXPECT_FALSE(pending3); // not done yet + EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[14]); + EXPECT_EQ(0x22, mspPackage.requestBuffer[15]); + EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]); + EXPECT_EQ(0x35, mspPackage.requestBuffer[17]); + EXPECT_EQ(0x19, mspPackage.requestBuffer[18]); + + const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4; + crsfFrame = *(const crsfFrame_t*)framePtr4; + crsfFrameDone = true; + uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + bool pending4 = handleMspFrame(frameStart4, frameEnd4); + EXPECT_FALSE(pending4); // not done yet + EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); + EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[21]); + EXPECT_EQ(0x32, mspPackage.requestBuffer[22]); + EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]); + EXPECT_EQ(0x28, mspPackage.requestBuffer[24]); + EXPECT_EQ(0x00, mspPackage.requestBuffer[25]); + //EXPECT_EQ(0xB3,checksum); + + const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5; + crsfFrame = *(const crsfFrame_t*)framePtr5; + crsfFrameDone = true; + uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; + bool pending5 = handleMspFrame(frameStart5, frameEnd5); + EXPECT_TRUE(pending5); // not done yet + EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); + EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); + EXPECT_EQ(0x37, mspPackage.requestBuffer[28]); + EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]); + EXPECT_EQ(0xF8,checksum); + +} + +void testSendMspResponse(uint8_t *payload) { + sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64); + sbufWriteData(plOut, payload, *payload + 64); + sbufSwitchToReader(&payloadOutputBuf, payloadOutput); +} + +TEST(CrossFireMSPTest, SendMspReply) { + initSharedMsp(); + const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest; + crsfFrame = *(const crsfFrame_t*)framePtr; + crsfFrameDone = true; + uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; + uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; + bool handled = handleMspFrame(frameStart, frameEnd); + EXPECT_TRUE(handled); + bool replyPending = sendMspReply(64, &testSendMspResponse); + EXPECT_FALSE(replyPending); + EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf)); + EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); + for (unsigned int ii=1; ii<=30; ii++) { + EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf)); + } + EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC +} + +// STUBS + +extern "C" { + + gpsSolutionData_t gpsSol; + attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; + + uint32_t micros(void) {return dummyTimeUs;} + serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;} + serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} + uint16_t getBatteryVoltage(void) { + return testBatteryVoltage; + } + int32_t getAmperage(void) { + return testAmperage; + } + + uint8_t calculateBatteryPercentageRemaining(void) { + return 67; + } + + bool feature(uint32_t) {return false;} + + bool isAirmodeActive(void) {return true;} + + mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { + + UNUSED(mspPostProcessFn); + + sbuf_t *dst = &reply->buf; + //sbuf_t *src = &cmd->buf; + const uint8_t cmdMSP = cmd->cmd; + reply->cmd = cmd->cmd; + + if (cmdMSP == 0x70) { + for (unsigned int ii=1; ii<=30; ii++) { + sbufWriteU8(dst, ii); + } + } else if (cmdMSP == 0xCA) { + return MSP_RESULT_ACK; + } + + return MSP_RESULT_ACK; + } + + void beeperConfirmationBeeps(uint8_t ) {} + + int32_t getMAhDrawn(void) { + return testmAhDrawn; + } + +} diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 2f92636d8c..0e46af9b92 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -31,6 +31,8 @@ extern "C" { #include "common/filter.h" #include "common/gps_conversion.h" #include "common/maths.h" + #include "common/printf.h" + #include "common/typeconversion.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -38,6 +40,7 @@ extern "C" { #include "drivers/serial.h" #include "drivers/system.h" + #include "fc/config.h" #include "fc/runtime_config.h" #include "flight/pid.h" @@ -53,6 +56,7 @@ extern "C" { #include "telemetry/crsf.h" #include "telemetry/telemetry.h" + #include "telemetry/msp_shared.h" bool airMode; @@ -63,6 +67,7 @@ extern "C" { serialPort_t *telemetrySharedPort; PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); } #include "unittest_macros.h" @@ -293,6 +298,7 @@ void serialWriteBuf(serialPort_t *, const uint8_t *, int) {} void serialSetMode(serialPort_t *, portMode_e) {} serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;} void closeSerialPort(serialPort_t *) {} +bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; } serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} @@ -323,4 +329,7 @@ int32_t getMAhDrawn(void){ return testmAhDrawn; } +bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } +bool handleMspFrame(uint8_t *, uint8_t *) { return false; } + } From f5d4dbfb0ea097f6d63e8c649e6f5cd34c0241b5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 10 Sep 2017 08:31:39 +0100 Subject: [PATCH 074/138] Undefine USE_DASHBOARD for SPRACINGF3EVO for ROM saving --- src/main/target/SPRACINGF3EVO/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index ffc531b982..92d883d86e 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -42,6 +42,7 @@ #endif #undef TELEMETRY_JETIEXBUS #undef USE_SERIALRX_JETIEXBUS +#undef USE_DASHBOARD #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT From 8a1ba09ee825d160ce2f1d00877d4ba9f0ee4ebd Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Sun, 10 Sep 2017 15:20:11 +0100 Subject: [PATCH 075/138] Test rendering of existing OSD power element --- src/test/unit/osd_unittest.cc | 62 +++++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index f45005fc4a..def9532ea0 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -604,6 +604,68 @@ TEST(OsdTest, TestElementMahDrawn) displayPortTestBufferSubstring(1, 11, "1042%c", SYM_MAH); } +/* + * Tests the instantaneous electrical power OSD element. + */ +TEST(OsdTest, TestElementPower) +{ + // given + osdConfigMutable()->item_pos[OSD_POWER] = OSD_POS(1, 10) | VISIBLE_FLAG; + + // and + simulationBatteryVoltage = 100; // 10V + + // and + simulationBatteryAmperage = 0; + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, "0W"); + + // given + simulationBatteryAmperage = 10; // 0.1AA + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, "1W"); + + // given + simulationBatteryAmperage = 120; // 1.2A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, "12W"); + + // given + simulationBatteryAmperage = 1230; // 12.3A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, "123W"); + + // given + simulationBatteryAmperage = 12340; // 123.4A + + // when + displayClearScreen(&testDisplayPort); + osdRefresh(simulationTime); + + // then + displayPortTestBufferSubstring(1, 10, "1234W"); +} + /* * Tests the altitude OSD element. */ From 30132299181435e6726fef548825aad34785d6da Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Sun, 10 Sep 2017 15:21:34 +0100 Subject: [PATCH 076/138] Left pad OSD electrical power readout --- src/main/io/osd.c | 2 +- src/test/unit/osd_unittest.cc | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a73d1ae7c3..f8478aa5c8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -572,7 +572,7 @@ static void osdDrawSingleElement(uint8_t item) } case OSD_POWER: - tfp_sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000); + tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000); break; case OSD_PIDRATE_PROFILE: diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index def9532ea0..b995f4c344 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -616,24 +616,24 @@ TEST(OsdTest, TestElementPower) simulationBatteryVoltage = 100; // 10V // and - simulationBatteryAmperage = 0; + simulationBatteryAmperage = 0; // 0A // when displayClearScreen(&testDisplayPort); osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(1, 10, "0W"); + displayPortTestBufferSubstring(1, 10, " 0W"); // given - simulationBatteryAmperage = 10; // 0.1AA + simulationBatteryAmperage = 10; // 0.1A // when displayClearScreen(&testDisplayPort); osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(1, 10, "1W"); + displayPortTestBufferSubstring(1, 10, " 1W"); // given simulationBatteryAmperage = 120; // 1.2A @@ -643,7 +643,7 @@ TEST(OsdTest, TestElementPower) osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(1, 10, "12W"); + displayPortTestBufferSubstring(1, 10, " 12W"); // given simulationBatteryAmperage = 1230; // 12.3A @@ -653,7 +653,7 @@ TEST(OsdTest, TestElementPower) osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(1, 10, "123W"); + displayPortTestBufferSubstring(1, 10, " 123W"); // given simulationBatteryAmperage = 12340; // 123.4A From 3bce54186c0f31972b8b1219820b10eb25f66777 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 11 Sep 2017 03:44:55 +1200 Subject: [PATCH 077/138] Disabled MSP over telemetry for F1, re-enabled SERVOS. --- src/main/rx/crsf.c | 2 ++ src/main/target/common_fc_post.h | 6 ++++++ src/main/target/common_fc_pre.h | 3 ++- src/main/telemetry/crsf.c | 10 +++++++++- src/main/telemetry/msp_shared.c | 2 +- src/main/telemetry/smartport.c | 9 +++++++++ src/main/telemetry/telemetry.c | 2 ++ src/test/Makefile | 15 +++++++++------ 8 files changed, 40 insertions(+), 9 deletions(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index f37cba0ae4..e06c6dc8d1 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -190,6 +190,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) // TODO: CRC CHECK scheduleDeviceInfoResponse(); return RX_FRAME_COMPLETE; +#if defined(USE_MSP_OVER_TELEMETRY) } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) { // TODO: CRC CHECK uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; @@ -198,6 +199,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) scheduleMspResponse(); } return RX_FRAME_COMPLETE; +#endif } } } diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 9fecb75121..19362d6766 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -50,3 +50,9 @@ #undef USE_BARO_MS5611 #endif #endif + +#if defined(USE_MSP_OVER_TELEMETRY) +#if !defined(TELEMETRY_SMARTPORT) && !defined(TELEMETRY_CRSF) +#undef USE_MSP_OVER_TELEMETRY +#endif +#endif diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 4f0c4165d5..faf9f078d8 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -102,6 +102,7 @@ #define TELEMETRY_LTM #define TELEMETRY_SMARTPORT #define USE_RESOURCE_MGMT +#define USE_SERVOS #endif #if (FLASH_SIZE > 128) @@ -117,7 +118,6 @@ #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS #define USE_SENSOR_NAMES -#define USE_SERVOS #define USE_VIRTUAL_CURRENT_METER #define VTX_COMMON #define VTX_CONTROL @@ -126,6 +126,7 @@ #define USE_CAMERA_CONTROL #define USE_HUFFMAN #define USE_COPY_PROFILE_CMS_MENU +#define USE_MSP_OVER_TELEMETRY #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c1deb636b3..2071ea404e 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -63,7 +63,9 @@ static bool crsfTelemetryEnabled; static bool deviceInfoReplyPending; +#if defined(USE_MSP_OVER_TELEMETRY) static bool mspReplyPending; +#endif static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; static void crsfInitializeFrame(sbuf_t *dst) @@ -280,6 +282,7 @@ typedef enum { static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +#if defined(USE_MSP_OVER_TELEMETRY) void scheduleMspResponse() { if (!mspReplyPending) { mspReplyPending = true; @@ -298,7 +301,8 @@ void crsfSendMspResponse(uint8_t *payload) sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE); crsfFinalize(dst); - } +} +#endif static void processCrsf(void) { @@ -341,7 +345,9 @@ void initCrsfTelemetry(void) crsfTelemetryEnabled = crsfRxIsActive(); deviceInfoReplyPending = false; +#if defined(USE_MSP_OVER_TELEMETRY) mspReplyPending = false; +#endif int index = 0; crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); @@ -384,8 +390,10 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) crsfFrameDeviceInfo(dst); crsfFinalize(dst); deviceInfoReplyPending = false; +#if defined(USE_MSP_OVER_TELEMETRY) } else if (mspReplyPending) { mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse); +#endif } else { processCrsf(); } diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 699571f3d7..29eb38b6d0 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -6,7 +6,7 @@ #include "platform.h" -#ifdef TELEMETRY +#if defined(USE_MSP_OVER_TELEMETRY) #include "build/build_config.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index e29e45fe2a..888d4cc485 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -169,7 +169,9 @@ typedef struct smartPortFrame_s { static smartPortFrame_t smartPortRxBuffer; static uint8_t smartPortRxBytes = 0; static bool smartPortFrameReceived = false; +#if defined(USE_MSP_OVER_TELEMETRY) static bool smartPortMspReplyPending = false; +#endif static void smartPortDataReceive(uint16_t c) { @@ -327,9 +329,11 @@ void checkSmartPortTelemetryState(void) freeSmartPortTelemetryPort(); } +#if defined(USE_MSP_OVER_TELEMETRY) void smartPortSendMspResponse(uint8_t *payload) { smartPortSendPackageEx(FSSP_MSPS_FRAME, payload); } +#endif void handleSmartPortTelemetry(void) { @@ -352,6 +356,8 @@ void handleSmartPortTelemetry(void) smartPortFrameReceived = false; // do not check the physical ID here again // unless we start receiving other sensors' packets + +#if defined(USE_MSP_OVER_TELEMETRY) if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { // Pass only the payload: skip sensorId & frameId @@ -361,6 +367,7 @@ void handleSmartPortTelemetry(void) smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd); } } +#endif } while (smartPortHasRequest) { @@ -370,11 +377,13 @@ void handleSmartPortTelemetry(void) return; } +#if defined(USE_MSP_OVER_TELEMETRY) if (smartPortMspReplyPending) { smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse); smartPortHasRequest = 0; return; } +#endif // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 9f9fe12663..369d8a38c4 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -101,7 +101,9 @@ void telemetryInit(void) #ifdef TELEMETRY_IBUS initIbusTelemetry(); #endif +#if defined(USE_MSP_OVER_TELEMETRY) initSharedMsp(); +#endif telemetryCheckState(); } diff --git a/src/test/Makefile b/src/test/Makefile index 7a18803e3c..96562f9eb7 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -214,6 +214,13 @@ telemetry_crsf_unittest_SRC := \ $(USER_DIR)/common/typeconversion.c \ $(USER_DIR)/fc/runtime_config.c +telemetry_crsf_unittest_DEFINES := \ + FLASH_SIZE=128 \ + STM32F10X_MD \ + __TARGET__="TEST" \ + __REVISION__="revision" + + telemetry_crsf_msp_unittest_SRC := \ $(USER_DIR)/rx/crsf.c \ $(USER_DIR)/common/crc.c \ @@ -227,12 +234,8 @@ telemetry_crsf_msp_unittest_SRC := \ $(USER_DIR)/telemetry/msp_shared.c \ $(USER_DIR)/fc/runtime_config.c - -telemetry_crsf_unittest_DEFINES := \ - FLASH_SIZE=128 \ - STM32F10X_MD \ - __TARGET__="TEST" \ - __REVISION__="revision" +telemetry_crsf_msp_unittest_DEFINES := \ + USE_MSP_OVER_TELEMETRY telemetry_hott_unittest_SRC := \ From aa1edbd97818311c5fb15ef9c2d37cd9c5a0ca6a Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 11 Sep 2017 03:01:51 +1200 Subject: [PATCH 078/138] Restored SmartPort MSP frame handling to how it was before the addition of MSP over CRSF. --- src/main/rx/crsf.c | 2 +- src/main/telemetry/msp_shared.c | 4 ++-- src/main/telemetry/msp_shared.h | 7 ++++++- src/main/telemetry/smartport.c | 10 ++++------ src/test/unit/rx_crsf_unittest.cc | 2 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 16 ++++++++-------- src/test/unit/telemetry_crsf_unittest.cc | 2 +- 7 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index f37cba0ae4..20a603a5ae 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -194,7 +194,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) // TODO: CRC CHECK uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - if(handleMspFrame(frameStart, frameEnd)) { + if(handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL)) { scheduleMspResponse(); } return RX_FRAME_COMPLETE; diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 699571f3d7..22640a6979 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -86,12 +86,12 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd) sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); } -bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling) { static uint8_t mspStarted = 0; static uint8_t lastSeq = 0; - if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { + if (handling != MSP_FRAME_HANDLING_FORCED && sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { return false; } diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index 315c47a615..3379d090c0 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -24,6 +24,11 @@ typedef union mspTxBuffer_u { uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; } mspTxBuffer_t; +typedef enum mspFrameHandling_e { + MSP_FRAME_HANDLING_NORMAL, + MSP_FRAME_HANDLING_FORCED +} mspFrameHandling_t; + void initSharedMsp(); -bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index e29e45fe2a..e66a58d06f 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -353,13 +353,11 @@ void handleSmartPortTelemetry(void) // do not check the physical ID here again // unless we start receiving other sensors' packets if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) { - // Pass only the payload: skip sensorId & frameId - if (!smartPortMspReplyPending) { - uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET; - uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE; - smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd); - } + uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET; + uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE; + + smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_FORCED); } } diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 09fa725b5f..f4d1258451 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -284,5 +284,5 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; void scheduleDeviceInfoResponse(void) {}; void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; -bool handleMspFrame(uint8_t *, uint8_t *) { return false; } +bool handleMspFrame(uint8_t *, uint8_t *, mspFrameHandling_t) { return false; } } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 8ef8099531..60d33ba756 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -60,7 +60,7 @@ extern "C" { #include "telemetry/msp_shared.h" #include "telemetry/smartport.h" - bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); + bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); uint8_t sbufReadU8(sbuf_t *src); int sbufBytesRemaining(sbuf_t *buf); @@ -130,7 +130,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest) crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - handleMspFrame(frameStart, frameEnd); + handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL); for (unsigned int ii=1; ii<30; ii++) { EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf)); } @@ -151,7 +151,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - bool pending1 = handleMspFrame(frameStart, frameEnd); + bool pending1 = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL); EXPECT_FALSE(pending1); // not done yet*/ EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); @@ -164,7 +164,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - bool pending2 = handleMspFrame(frameStart2, frameEnd2); + bool pending2 = handleMspFrame(frameStart2, frameEnd2, MSP_FRAME_HANDLING_NORMAL); EXPECT_FALSE(pending2); // not done yet EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); @@ -179,7 +179,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - bool pending3 = handleMspFrame(frameStart3, frameEnd3); + bool pending3 = handleMspFrame(frameStart3, frameEnd3, MSP_FRAME_HANDLING_NORMAL); EXPECT_FALSE(pending3); // not done yet EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); @@ -194,7 +194,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - bool pending4 = handleMspFrame(frameStart4, frameEnd4); + bool pending4 = handleMspFrame(frameStart4, frameEnd4, MSP_FRAME_HANDLING_NORMAL); EXPECT_FALSE(pending4); // not done yet EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); @@ -210,7 +210,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - bool pending5 = handleMspFrame(frameStart5, frameEnd5); + bool pending5 = handleMspFrame(frameStart5, frameEnd5, MSP_FRAME_HANDLING_NORMAL); EXPECT_TRUE(pending5); // not done yet EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); @@ -233,7 +233,7 @@ TEST(CrossFireMSPTest, SendMspReply) { crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - bool handled = handleMspFrame(frameStart, frameEnd); + bool handled = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL); EXPECT_TRUE(handled); bool replyPending = sendMspReply(64, &testSendMspResponse); EXPECT_FALSE(replyPending); diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 0e46af9b92..38374167d6 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -330,6 +330,6 @@ int32_t getMAhDrawn(void){ } bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } -bool handleMspFrame(uint8_t *, uint8_t *) { return false; } +bool handleMspFrame(uint8_t *, uint8_t *, mspFrameHandling_t) { return false; } } From 5ccd39843d3e80ef4a918b7d7d4d78a06104f99f Mon Sep 17 00:00:00 2001 From: Curtis Bangert Date: Sun, 10 Sep 2017 14:17:31 -0400 Subject: [PATCH 079/138] Reset msp handling upon receipt of new incoming frames in msp_shared --- src/main/rx/crsf.c | 2 +- src/main/telemetry/crsf.c | 4 +--- src/main/telemetry/msp_shared.c | 6 +++--- src/main/telemetry/msp_shared.h | 7 +------ src/main/telemetry/smartport.c | 2 +- src/test/unit/rx_crsf_unittest.cc | 2 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 16 ++++++++-------- src/test/unit/telemetry_crsf_unittest.cc | 2 +- 8 files changed, 17 insertions(+), 24 deletions(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 443a402217..e06c6dc8d1 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -195,7 +195,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) // TODO: CRC CHECK uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - if(handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL)) { + if(handleMspFrame(frameStart, frameEnd)) { scheduleMspResponse(); } return RX_FRAME_COMPLETE; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 2071ea404e..d7d18bf692 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -284,9 +284,7 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) void scheduleMspResponse() { - if (!mspReplyPending) { - mspReplyPending = true; - } + mspReplyPending = true; } void crsfSendMspResponse(uint8_t *payload) diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index fa1015b1ac..074b9f92b0 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -86,13 +86,13 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd) sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); } -bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling) +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) { static uint8_t mspStarted = 0; static uint8_t lastSeq = 0; - if (handling != MSP_FRAME_HANDLING_FORCED && sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { - return false; + if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { + mspStarted = 0; } if (mspStarted == 0) { diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index 3379d090c0..315c47a615 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -24,11 +24,6 @@ typedef union mspTxBuffer_u { uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; } mspTxBuffer_t; -typedef enum mspFrameHandling_e { - MSP_FRAME_HANDLING_NORMAL, - MSP_FRAME_HANDLING_FORCED -} mspFrameHandling_t; - void initSharedMsp(); -bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling); +bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index a6b7261423..0ecfaba1c3 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -363,7 +363,7 @@ void handleSmartPortTelemetry(void) uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET; uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE; - smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_FORCED); + smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd); } #endif } diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index f4d1258451..09fa725b5f 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -284,5 +284,5 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; void scheduleDeviceInfoResponse(void) {}; void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; -bool handleMspFrame(uint8_t *, uint8_t *, mspFrameHandling_t) { return false; } +bool handleMspFrame(uint8_t *, uint8_t *) { return false; } } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 60d33ba756..8ef8099531 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -60,7 +60,7 @@ extern "C" { #include "telemetry/msp_shared.h" #include "telemetry/smartport.h" - bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd, mspFrameHandling_t handling); + bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); uint8_t sbufReadU8(sbuf_t *src); int sbufBytesRemaining(sbuf_t *buf); @@ -130,7 +130,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest) crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL); + handleMspFrame(frameStart, frameEnd); for (unsigned int ii=1; ii<30; ii++) { EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf)); } @@ -151,7 +151,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - bool pending1 = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL); + bool pending1 = handleMspFrame(frameStart, frameEnd); EXPECT_FALSE(pending1); // not done yet*/ EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); @@ -164,7 +164,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - bool pending2 = handleMspFrame(frameStart2, frameEnd2, MSP_FRAME_HANDLING_NORMAL); + bool pending2 = handleMspFrame(frameStart2, frameEnd2); EXPECT_FALSE(pending2); // not done yet EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); @@ -179,7 +179,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - bool pending3 = handleMspFrame(frameStart3, frameEnd3, MSP_FRAME_HANDLING_NORMAL); + bool pending3 = handleMspFrame(frameStart3, frameEnd3); EXPECT_FALSE(pending3); // not done yet EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); @@ -194,7 +194,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - bool pending4 = handleMspFrame(frameStart4, frameEnd4, MSP_FRAME_HANDLING_NORMAL); + bool pending4 = handleMspFrame(frameStart4, frameEnd4); EXPECT_FALSE(pending4); // not done yet EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); @@ -210,7 +210,7 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - bool pending5 = handleMspFrame(frameStart5, frameEnd5, MSP_FRAME_HANDLING_NORMAL); + bool pending5 = handleMspFrame(frameStart5, frameEnd5); EXPECT_TRUE(pending5); // not done yet EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); @@ -233,7 +233,7 @@ TEST(CrossFireMSPTest, SendMspReply) { crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2; - bool handled = handleMspFrame(frameStart, frameEnd, MSP_FRAME_HANDLING_NORMAL); + bool handled = handleMspFrame(frameStart, frameEnd); EXPECT_TRUE(handled); bool replyPending = sendMspReply(64, &testSendMspResponse); EXPECT_FALSE(replyPending); diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 38374167d6..2744947275 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -330,6 +330,6 @@ int32_t getMAhDrawn(void){ } bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } -bool handleMspFrame(uint8_t *, uint8_t *, mspFrameHandling_t) { return false; } +bool handleMspFrame(uint8_t *, uint8_t *) { return false; } } From 992403d2dd6a0d007dbe405d5f11894d13dae367 Mon Sep 17 00:00:00 2001 From: Spencer Owen Date: Sun, 10 Sep 2017 21:06:25 -0600 Subject: [PATCH 080/138] Shows MSP api version in cli --- src/main/build/version.h | 2 ++ src/main/fc/cli.c | 7 +++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index b5cc9e7e72..d03f86fa39 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -36,3 +36,5 @@ extern const char* const buildDate; // "MMM DD YYYY" MMM = Jan/Feb/... #define BUILD_TIME_LENGTH 8 extern const char* const buildTime; // "HH:MM:SS" + +#define MSP_API_VERSION_STRING STR(API_VERSION_MAJOR) "." STR(API_VERSION_MINOR) \ No newline at end of file diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d969a61685..bb6236d9db 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -114,6 +114,8 @@ extern uint8_t __config_end; #include "io/vtx_rtc6705.h" #include "io/vtx_control.h" +#include "msp/msp_protocol.h" + #include "rx/rx.h" #include "rx/spektrum.h" #include "rx/frsky_d.h" @@ -3079,14 +3081,15 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintLinef("# %s / %s (%s) %s %s / %s (%s)", + cliPrintLinef("# %s / %s (%s) %s %s / %s (%s) MSP API: %s", FC_FIRMWARE_NAME, targetName, systemConfig()->boardIdentifier, FC_VERSION_STRING, buildDate, buildTime, - shortGitRevision + shortGitRevision, + MSP_API_VERSION_STRING ); } From 605b3e14e5f43fe0b8ca2733dcb94eb9a607929a Mon Sep 17 00:00:00 2001 From: Spencer Owen Date: Mon, 11 Sep 2017 09:05:02 -0600 Subject: [PATCH 081/138] Adds sync commands --- Vagrantfile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Vagrantfile b/Vagrantfile index 044a1be33e..3c337f53a2 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -16,6 +16,10 @@ Vagrant.configure(2) do |config| config.vm.provider "virtualbox" do |v| v.memory = 4096 + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-interval", 10000] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-min-adjust", 100] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", 1] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-threshold", 1000] end # Enable provisioning with a shell script. Additional provisioners such as From 5bf7ee6cb5179f4719c90f37dd30b6555c409293 Mon Sep 17 00:00:00 2001 From: Spencer Owen Date: Mon, 11 Sep 2017 09:07:51 -0600 Subject: [PATCH 082/138] Increase threshold for time sync --- Vagrantfile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index 3c337f53a2..f21fcf0a90 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -19,7 +19,7 @@ Vagrant.configure(2) do |config| v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-interval", 10000] v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-min-adjust", 100] v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", 1] - v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-threshold", 1000] + v.customize [ "guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-threshold", 10000] end # Enable provisioning with a shell script. Additional provisioners such as @@ -30,8 +30,7 @@ Vagrant.configure(2) do |config| add-apt-repository ppa:team-gcc-arm-embedded/ppa apt-get update apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1 - apt-get install -y make python gcc clang ntp - timedatectl set-ntp true + apt-get install -y make python gcc clang SHELL end From 3aa171ed479bb350f861f0a4a56da75a9cf9ec98 Mon Sep 17 00:00:00 2001 From: Andreas Bertheussen Date: Mon, 11 Sep 2017 20:56:16 +0200 Subject: [PATCH 083/138] Set Betaflight F3 current scale default back to 220 Fixes mistake from commit c59e7312c246aa2b3544ec1d909e1d7ca2079569 in PR #2699 where current scale was set to 200. --- src/main/target/BETAFLIGHTF3/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 6501394508..bb8389396b 100644 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -117,7 +117,7 @@ #define CURRENT_METER_ADC_PIN PA5 #define RSSI_ADC_PIN PB2 -#define CURRENT_METER_SCALE_DEFAULT 200 +#define CURRENT_METER_SCALE_DEFAULT 220 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT From 004e98457f22054c8cbd56253004ef2f8cd28be0 Mon Sep 17 00:00:00 2001 From: jirif Date: Mon, 11 Sep 2017 21:19:26 +0200 Subject: [PATCH 084/138] "diff|dump all" export can be used as direct copy/paste import "defaults" command causing reboot of the FC and ignoring remaining part of the pasted commands from "diff|dump all" exports. This change adding "nosave" parameter/functionality for the "defaults" command and "diff|dump all" exports. This parameter will not save eeprom and do not reboot the FC. --- src/main/fc/cli.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index bb6236d9db..9213a4e396 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2762,11 +2762,11 @@ static void cliSave(char *cmdline) static void cliDefaults(char *cmdline) { - UNUSED(cmdline); - cliPrintHashLine("resetting to defaults"); - resetEEPROM(); - cliReboot(); + resetConfigs(); + if (isEmpty(cmdline)) { + cliSave(NULL); + } } STATIC_UNIT_TESTED void cliGet(char *cmdline) @@ -3435,7 +3435,7 @@ static void printConfig(char *cmdline, bool doDiff) if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { cliPrintHashLine("reset configuration to default settings"); - cliPrint("defaults"); + cliPrint("defaults nosave"); cliPrintLinefeed(); } @@ -3600,7 +3600,7 @@ const clicmd_t cmdTable[] = { #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif - CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), + CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", "[nosave]", cliDefaults), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|profile|rates|all] {defaults}", cliDiff), #ifdef USE_DSHOT From 96c7ef22c860776a2af06a3f8d71daaf8a90c976 Mon Sep 17 00:00:00 2001 From: jirif Date: Tue, 12 Sep 2017 10:05:45 +0200 Subject: [PATCH 085/138] add strict argument checking for the "defaults" cli command --- src/main/fc/cli.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9213a4e396..ccb95f36a1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2762,9 +2762,19 @@ static void cliSave(char *cmdline) static void cliDefaults(char *cmdline) { + bool saveConfigs; + + if (isEmpty(cmdline)) { + saveConfigs = true; + } else if (strncasecmp(cmdline, "nosave", 6) == 0) { + saveConfigs = false; + } else { + return; + } + cliPrintHashLine("resetting to defaults"); resetConfigs(); - if (isEmpty(cmdline)) { + if (saveConfigs) { cliSave(NULL); } } From 6edb60b48c4eabbc83af0336783067c71ca168a8 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 12 Sep 2017 22:51:21 +1200 Subject: [PATCH 086/138] Fixed Spektrum binding when overclocked. --- src/main/fc/fc_init.c | 28 ++++++++++++++-------------- src/main/fc/settings.c | 2 +- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 27c4c56c57..121f71adeb 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -273,20 +273,6 @@ void init(void) resetEEPROM(); } -#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) - // If F4 Overclocking is set and System core clock is not correct a reset is forced - if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } - -#endif - systemState |= SYSTEM_STATE_CONFIG_LOADED; //i2cSetOverclock(masterConfig.i2c_overclock); @@ -351,6 +337,20 @@ void init(void) } #endif +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // If F4 Overclocking is set and System core clock is not correct a reset is forced + if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } + +#endif + delay(100); timerInit(); // timer must be initialized before any channel is allocated diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 280319f8a3..4143b9d68b 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -373,7 +373,7 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, - { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, + { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, #endif { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, From f598145b48644b0dd36ed8a15db1903b33a1119e Mon Sep 17 00:00:00 2001 From: atomiclama Date: Tue, 12 Sep 2017 13:33:46 +0100 Subject: [PATCH 087/138] Initialise rtc6705 CS to high. Currently the pin initialisation sets it low, this allows any initialisation glitches on the DATA and CLK lines to possibly be detected. This PR changes the order and forces the CS high before any changes to the DATA and CLK lines. I know there is information out there that the rtc6705 ignores the CS but as we are using it lets do it properly. --- src/main/drivers/vtx_rtc6705_soft_spi.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c index 2bffc9f6cc..bc66ae2001 100644 --- a/src/main/drivers/vtx_rtc6705_soft_spi.c +++ b/src/main/drivers/vtx_rtc6705_soft_spi.c @@ -65,11 +65,12 @@ void rtc6705IOInit(void) rtc6705LePin = IOGetByTag(IO_TAG(RTC6705_SPILE_PIN)); rtc6705ClkPin = IOGetByTag(IO_TAG(RTC6705_SPICLK_PIN)); - IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET); - IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); - IOInit(rtc6705LePin, OWNER_SPI_CS, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705LePin, IOCFG_OUT_PP); + RTC6705_SPILE_ON; + + IOInit(rtc6705DataPin, OWNER_SPI_MOSI, RESOURCE_SOFT_OFFSET); + IOConfigGPIO(rtc6705DataPin, IOCFG_OUT_PP); IOInit(rtc6705ClkPin, OWNER_SPI_SCK, RESOURCE_SOFT_OFFSET); IOConfigGPIO(rtc6705ClkPin, IOCFG_OUT_PP); From 7514c4ca26e6a480e42a04d3a760354f5cf01278 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 12 Sep 2017 23:54:17 +1200 Subject: [PATCH 088/138] Restrict LED_STRIP fading to modes where it acutally works. --- src/main/io/ledstrip.c | 43 +++++++++++++++++++++--------------------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index eb5d1840f6..2f6437ef28 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -446,17 +446,30 @@ static void applyLedFixedLayers() for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex]; hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); - hsvColor_t nextColor = *getSC(LED_SCOLOR_BACKGROUND); //next color above the one selected, or color 0 if your are at the maximum - hsvColor_t previousColor = *getSC(LED_SCOLOR_BACKGROUND); //Previous color to the one selected, modulo color count int fn = ledGetFunction(ledConfig); - int hOffset = HSV_HUE_MAX; + int hOffset = HSV_HUE_MAX + 1; switch (fn) { case LED_FUNCTION_COLOR: color = ledStripConfig()->colors[ledGetColor(ledConfig)]; - nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; - previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + + hsvColor_t nextColor = ledStripConfig()->colors[(ledGetColor(ledConfig) + 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + hsvColor_t previousColor = ledStripConfig()->colors[(ledGetColor(ledConfig) - 1 + LED_CONFIGURABLE_COLOR_COUNT) % LED_CONFIGURABLE_COLOR_COUNT]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor + int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2; + if (auxInput < centerPWM) { + color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h); + color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s); + color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v); + } else { + color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h); + color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s); + color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v); + } + } + break; case LED_FUNCTION_FLIGHT_MODE: @@ -489,22 +502,10 @@ static void applyLedFixedLayers() break; } - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) //smooth fade with selected Aux channel of all HSV values from previousColor through color to nextColor - { - int centerPWM = (PWM_RANGE_MIN + PWM_RANGE_MAX) / 2; - if (auxInput < centerPWM) - { - color.h = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.h, color.h); - color.s = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.s, color.s); - color.v = scaleRange(auxInput, PWM_RANGE_MIN, centerPWM, previousColor.v, color.v); - } - else - { - color.h = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.h, nextColor.h); - color.s = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.s, nextColor.s); - color.v = scaleRange(auxInput, centerPWM, PWM_RANGE_MAX, color.v, nextColor.v); - } - } + if ((fn != LED_FUNCTION_COLOR) && ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { + hOffset += scaleRange(auxInput, PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1); + } + color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); setLedHsv(ledIndex, &color); } From 30816c9aa60ad9e918b14e0e5c802a050cb3b115 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 13 Sep 2017 22:14:40 +1200 Subject: [PATCH 089/138] Revert "Fixed Spektrum binding when overclocked." --- src/main/fc/fc_init.c | 28 ++++++++++++++-------------- src/main/fc/settings.c | 2 +- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 121f71adeb..27c4c56c57 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -273,6 +273,20 @@ void init(void) resetEEPROM(); } +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // If F4 Overclocking is set and System core clock is not correct a reset is forced + if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } + +#endif + systemState |= SYSTEM_STATE_CONFIG_LOADED; //i2cSetOverclock(masterConfig.i2c_overclock); @@ -337,20 +351,6 @@ void init(void) } #endif -#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) - // If F4 Overclocking is set and System core clock is not correct a reset is forced - if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } - -#endif - delay(100); timerInit(); // timer must be initialized before any channel is allocated diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 4143b9d68b..280319f8a3 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -373,7 +373,7 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, - { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, + { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, #endif { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, From 0e19f7701d73f0212d6b1936bafb881520cf548d Mon Sep 17 00:00:00 2001 From: jirif Date: Thu, 14 Sep 2017 12:16:52 +0200 Subject: [PATCH 090/138] BEEPER_USB flag will silence the beeper only when battery is really not present (#4121) Partial fox for #4107 --- src/main/io/beeper.c | 2 +- src/main/sensors/battery.c | 9 +++++---- src/main/sensors/battery.h | 3 ++- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 4f803b430a..5ce73ff074 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -224,7 +224,7 @@ void beeper(beeperMode_e mode) if ( mode == BEEPER_SILENCE || ( (getBeeperOffMask() & (1 << (BEEPER_USB - 1))) - && (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && (getBatteryCellCount() == 0)) + && getBatteryState() == BATTERY_NOT_PRESENT ) ) { beeperSilence(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index a0320b3cd4..ed479b0baf 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -154,6 +154,7 @@ static void updateBatteryBeeperAlert(void) break; case BATTERY_OK: case BATTERY_NOT_PRESENT: + case BATTERY_INIT: break; } } @@ -168,7 +169,7 @@ void batteryUpdatePresence(void) if ( - voltageState == BATTERY_NOT_PRESENT + (voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat && isVoltageStable ) { @@ -275,7 +276,7 @@ batteryState_e getBatteryState(void) return batteryState; } -const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT"}; +const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT", "INIT"}; const char * getBatteryStateString(void) { @@ -287,13 +288,13 @@ void batteryInit(void) // // presence // - batteryState = BATTERY_NOT_PRESENT; + batteryState = BATTERY_INIT; batteryCellCount = 0; // // voltage // - voltageState = BATTERY_NOT_PRESENT; + voltageState = BATTERY_INIT; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; lowVoltageCutoff.enabled = false; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 467b229329..27744df495 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -59,7 +59,8 @@ typedef enum { BATTERY_OK = 0, BATTERY_WARNING, BATTERY_CRITICAL, - BATTERY_NOT_PRESENT + BATTERY_NOT_PRESENT, + BATTERY_INIT } batteryState_e; void batteryInit(void); From f4af2da6e82b13787e2830ff79c315f39124d449 Mon Sep 17 00:00:00 2001 From: awolf78 Date: Thu, 14 Sep 2017 06:22:07 -0400 Subject: [PATCH 091/138] Added MSP_EXTRA_ESC_DATA MSP (#4140) --- src/main/fc/fc_msp.c | 11 +++++++++++ src/main/msp/msp_protocol.h | 1 + 2 files changed, 12 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index aae0ec773c..8869842cb8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -108,6 +108,7 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" #include "sensors/sonar.h" +#include "sensors/esc_sensor.h" #include "telemetry/telemetry.h" @@ -930,6 +931,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) break; #endif +#ifdef USE_DSHOT + case MSP_ESC_SENSOR_DATA: + sbufWriteU8(dst, getMotorCount()); + for (int i = 0; i < getMotorCount(); i++) { + sbufWriteU8(dst, getEscSensorData(i)->temperature); + sbufWriteU16(dst, getEscSensorData(i)->rpm); + } + break; +#endif + #ifdef GPS case MSP_GPS_CONFIG: sbufWriteU8(dst, gpsConfig()->provider); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index fbcc14ee4f..e74709330d 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -271,6 +271,7 @@ #define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc) #define MSP_GPS_CONFIG 132 //out message GPS configuration #define MSP_COMPASS_CONFIG 133 //out message Compass configuration +#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed From 811a78c5e14a92b95006775992bf67c07b5c3f2e Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Thu, 14 Sep 2017 17:57:46 +0100 Subject: [PATCH 092/138] Fix define dependencies for VTX code Fixes #4147 --- src/main/cms/cms_menu_vtx_smartaudio.c | 2 +- src/main/cms/cms_menu_vtx_tramp.c | 3 ++- src/main/target/common_fc_post.h | 8 ++++++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index 34a8a713cc..6636e8b4ab 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -21,7 +21,7 @@ #include "platform.h" -#ifdef CMS +#if defined(CMS) && defined(VTX_SMARTAUDIO) #include "common/printf.h" #include "common/utils.h" diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index 5265c8dd22..b177274449 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -21,7 +21,8 @@ #include "platform.h" -#ifdef CMS +#if defined(CMS) && defined(VTX_TRAMP) + #include "common/printf.h" #include "common/utils.h" diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 19362d6766..ac3285cbad 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -56,3 +56,11 @@ #undef USE_MSP_OVER_TELEMETRY #endif #endif + +/* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */ +#if !defined(VTX_COMMON) || !defined(VTX_CONTROL) +#undef VTX_COMMON +#undef VTX_CONTROL +#undef VTX_TRAMP +#undef VTX_SMARTAUDIO +#endif From 5a7ed7b686decb8f44acb5a3abe5ec8ea38a7bc3 Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 15 Sep 2017 11:10:28 +0900 Subject: [PATCH 093/138] Handle SPI clock for CPU over clocked environment --- src/main/config/parameter_group_ids.h | 3 +- src/main/drivers/max7456.c | 65 ++++++++++++++++++++++++++- src/main/drivers/max7456.h | 10 +++++ src/main/fc/settings.c | 6 +++ src/main/target/OMNIBUSF4/config.c | 35 +++++++++++++++ src/main/target/OMNIBUSF4/target.h | 2 + 6 files changed, 119 insertions(+), 2 deletions(-) create mode 100644 src/main/target/OMNIBUSF4/config.c diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index ea95a6a80e..603cf78be1 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -114,7 +114,8 @@ #define PG_ESCSERIAL_CONFIG 521 #define PG_CAMERA_CONTROL_CONFIG 522 #define PG_FRSKY_D_CONFIG 523 -#define PG_BETAFLIGHT_END 523 +#define PG_MAX7456_CONFIG 524 +#define PG_BETAFLIGHT_END 524 // OSD configuration (subject to change) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index f2abc383fa..3ac56db359 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -24,8 +24,13 @@ #ifdef USE_MAX7456 +#include "build/debug.h" + #include "common/printf.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/bus_spi.h" #include "drivers/dma.h" #include "drivers/io.h" @@ -36,6 +41,8 @@ #include "drivers/time.h" #include "drivers/vcd.h" +#include "fc/config.h" // For systemConfig() + // VM0 bits #define VIDEO_BUFFER_DISABLE 0x01 #define MAX7456_RESET 0x02 @@ -148,6 +155,10 @@ #define NVM_RAM_SIZE 54 #define WRITE_NVR 0xA0 +// Device type +#define MAX7456_DEVICE_TYPE_MAX 0 +#define MAX7456_DEVICE_TYPE_AT 1 + #define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*? // On shared SPI buss we want to change clock for OSD chip and restore for other devices. @@ -164,6 +175,12 @@ #define DISABLE_MAX7456 IOHi(max7456CsPin) #endif +#ifndef MAX7456_SPI_CLK +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) +#endif + +static uint16_t max7456SpiClock = MAX7456_SPI_CLK; + uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; // We write everything in screenBuffer and then compare @@ -193,6 +210,15 @@ static bool max7456Lock = false; static bool fontIsLoading = false; static IO_t max7456CsPin = IO_NONE; +static uint8_t max7456DeviceType; + + +PG_REGISTER_WITH_RESET_TEMPLATE(max7456Config_t, max7456Config, PG_MAX7456_CONFIG, 0); + +PG_RESET_TEMPLATE(max7456Config_t, max7456Config, + .clockConfig = MAX7456_CLOCK_CONFIG_OC, // SPI clock based on device type and overclock state +); + static uint8_t max7456Send(uint8_t add, uint8_t data) { @@ -387,6 +413,7 @@ void max7456ReInit(void) // Here we init only CS and try to init MAX for first time. +// Also detect device type (MAX v.s. AT) void max7456Init(const vcdProfile_t *pVcdProfile) { @@ -399,7 +426,43 @@ void max7456Init(const vcdProfile_t *pVcdProfile) IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); IOHi(max7456CsPin); - spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); + // Detect device type by writing and reading CA[8] bit at CMAL[6]. + // Do this at half the speed for safety. + spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK * 2); + + max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit + + if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) { + max7456DeviceType = MAX7456_DEVICE_TYPE_AT; + } else { + max7456DeviceType = MAX7456_DEVICE_TYPE_MAX; + } + +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // Determine SPI clock divisor based on config and the device type. + + switch (max7456Config()->clockConfig) { + case MAX7456_CLOCK_CONFIG_HALF: + max7456SpiClock = MAX7456_SPI_CLK * 2; + break; + + case MAX7456_CLOCK_CONFIG_OC: + max7456SpiClock = (systemConfig()->cpu_overclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK; + break; + + case MAX7456_CLOCK_CONFIG_FULL: + max7456SpiClock = MAX7456_SPI_CLK; + break; + } + + // XXX Disable for production + debug[0] = systemConfig()->cpu_overclock; + debug[1] = max7456DeviceType; + debug[2] = max7456SpiClock; +#endif + + spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock); + // force soft reset on Max7456 ENABLE_MAX7456; max7456Send(MAX7456ADD_VM0, MAX7456_RESET); diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 8e74d46fab..63cf0e15ff 100755 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -48,3 +48,13 @@ void max7456ClearScreen(void); void max7456RefreshAll(void); uint8_t* max7456GetScreenBuffer(void); bool max7456DmaInProgress(void); + +typedef struct max7456Config_s { + uint8_t clockConfig; // 0 = force half clock, 1 = half if OC, 2 = force full +} max7456Config_t; + +#define MAX7456_CLOCK_CONFIG_HALF 0 +#define MAX7456_CLOCK_CONFIG_OC 1 +#define MAX7456_CLOCK_CONFIG_FULL 2 + +PG_DECLARE(max7456Config_t, max7456Config); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 280319f8a3..e9e6ceefcc 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -38,6 +38,7 @@ #include "drivers/light_led.h" #include "drivers/camera_control.h" +#include "drivers/max7456.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -741,6 +742,11 @@ const clivalue_t valueTable[] = { { "vcd_v_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -15, 16 }, PG_VCD_CONFIG, offsetof(vcdProfile_t, v_offset) }, #endif +// PG_MAX7456_CONFIG +#ifdef USE_MAX7456 + { "max7456_clock", VAR_UINT8| MASTER_VALUE, .config.minmax = { 0, 2 }, PG_MAX7456_CONFIG, offsetof(max7456Config_t, clockConfig) }, +#endif + // PG_DISPLAY_PORT_MSP_CONFIG #ifdef USE_MSP_DISPLAYPORT { "displayport_msp_col_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) }, diff --git a/src/main/target/OMNIBUSF4/config.c b/src/main/target/OMNIBUSF4/config.c new file mode 100644 index 0000000000..43a1d60d96 --- /dev/null +++ b/src/main/target/OMNIBUSF4/config.c @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include + +#ifdef TARGET_CONFIG + +#include "config/parameter_group.h" +#include "drivers/max7456.h" + +void targetConfiguration(void) +{ +#ifdef OMNIBUSF4 +// OMNIBUS F4 AIO (1st gen) has a AB7456 chip that is detected as MAX7456 + max7456ConfigMutable()->clockConfig = MAX7456_CLOCK_CONFIG_FULL; +#endif +} +#endif diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index f2bbd61996..a05cd878f0 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -15,6 +15,8 @@ #pragma once +#define TARGET_CONFIG + #if defined(OMNIBUSF4SD) #define TARGET_BOARD_IDENTIFIER "OBSD" #elif defined(LUXF4OSD) From 365a97cc8e63898de7d1ac09b1c7b1697d2b20dc Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 15 Sep 2017 14:42:51 +0900 Subject: [PATCH 094/138] Use keywords for max7456_clock value --- src/main/fc/settings.c | 11 ++++++++++- src/main/fc/settings.h | 3 +++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index e9e6ceefcc..569fa78106 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -250,6 +250,12 @@ static const char * const lookupTableBusType[] = { "NONE", "I2C", "SPI" }; +#ifdef USE_MAX7456 +static const char * const lookupTableMax7456Clock[] = { + "HALF", "DEFAULT", "FULL" +}; +#endif + const lookupTableEntry_t lookupTables[] = { { lookupTableOffOn, sizeof(lookupTableOffOn) / sizeof(char *) }, { lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) }, @@ -296,6 +302,9 @@ const lookupTableEntry_t lookupTables[] = { { lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) }, #endif { lookupTableBusType, sizeof(lookupTableBusType) / sizeof(char *) }, +#ifdef USE_MAX7456 + { lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) }, +#endif }; const clivalue_t valueTable[] = { @@ -744,7 +753,7 @@ const clivalue_t valueTable[] = { // PG_MAX7456_CONFIG #ifdef USE_MAX7456 - { "max7456_clock", VAR_UINT8| MASTER_VALUE, .config.minmax = { 0, 2 }, PG_MAX7456_CONFIG, offsetof(max7456Config_t, clockConfig) }, + { "max7456_clock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAX7456_CLOCK }, PG_MAX7456_CONFIG, offsetof(max7456Config_t, clockConfig) }, #endif // PG_DISPLAY_PORT_MSP_CONFIG diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index d82fc440b2..7283bb4f27 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -68,6 +68,9 @@ typedef enum { TABLE_CAMERA_CONTROL_MODE, #endif TABLE_BUS_TYPE, +#ifdef USE_MAX7456 + TABLE_MAX7456_CLOCK, +#endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; From a9d56097d1b3466b5f4b4ba10a13cbb8db192c0b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 15 Sep 2017 09:31:37 +0100 Subject: [PATCH 095/138] Renamed CLI setting blackbox_p_denom to blackbox_p_ratio --- src/main/blackbox/blackbox.c | 2 +- src/main/fc/settings.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9d42a54435..bbf149ce38 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1701,7 +1701,7 @@ void blackboxInit(void) // by default p_denom is 32 and a P-frame is written every 1ms // if p_denom is zero then no P-frames are logged if (blackboxConfig()->p_denom == 0) { - blackboxPInterval = 0; + blackboxPInterval = 0; // blackboxPInterval not used when p_denom is zero, so just set it to zero } else if (blackboxConfig()->p_denom > blackboxIInterval && blackboxIInterval >= 32) { blackboxPInterval = 1; } else { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 280319f8a3..9fbdc1073e 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -392,7 +392,7 @@ const clivalue_t valueTable[] = { // PG_BLACKBOX_CONFIG #ifdef BLACKBOX - { "blackbox_p_denom", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) }, + { "blackbox_p_ratio", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, INT16_MAX }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, p_denom) }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BLACKBOX_DEVICE }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, device) }, { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, on_motor_test) }, { "blackbox_record_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BLACKBOX_CONFIG, offsetof(blackboxConfig_t, record_acc) }, From 21175320b4c3daca4dea8c50c62f183ecdee5e3a Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 15 Sep 2017 21:57:55 +1200 Subject: [PATCH 096/138] Fixed Spektrum binding when overclocked. Also changed data type of 'spektrum_sat_bind_autoreset' from uint8_t to lookup(TABLE_OFF_ON). --- src/main/fc/fc_init.c | 28 ++++++++++++++-------------- src/main/fc/settings.c | 2 +- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 27c4c56c57..121f71adeb 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -273,20 +273,6 @@ void init(void) resetEEPROM(); } -#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) - // If F4 Overclocking is set and System core clock is not correct a reset is forced - if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { - *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX - __disable_irq(); - NVIC_SystemReset(); - } - -#endif - systemState |= SYSTEM_STATE_CONFIG_LOADED; //i2cSetOverclock(masterConfig.i2c_overclock); @@ -351,6 +337,20 @@ void init(void) } #endif +#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK) + // If F4 Overclocking is set and System core clock is not correct a reset is forced + if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) { + *((uint32_t *)0x2001FFF8) = 0x0; // 128KB SRAM STM32F4XX + __disable_irq(); + NVIC_SystemReset(); + } + +#endif + delay(100); timerInit(); // timer must be initialized before any channel is allocated diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 9fbdc1073e..b32f1c278f 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -373,7 +373,7 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, - { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, + { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, #endif { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, From 8534d565c7481d450331f2f9a934ed621873545b Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 15 Sep 2017 23:50:31 +0900 Subject: [PATCH 097/138] Define OMNIBUSF4BASE to single out the Gen1 board --- src/main/target/OMNIBUSF4/config.c | 4 ++-- src/main/target/OMNIBUSF4/target.h | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/OMNIBUSF4/config.c b/src/main/target/OMNIBUSF4/config.c index 43a1d60d96..627743767d 100644 --- a/src/main/target/OMNIBUSF4/config.c +++ b/src/main/target/OMNIBUSF4/config.c @@ -27,8 +27,8 @@ void targetConfiguration(void) { -#ifdef OMNIBUSF4 -// OMNIBUS F4 AIO (1st gen) has a AB7456 chip that is detected as MAX7456 +#ifdef OMNIBUSF4BASE + // OMNIBUS F4 AIO (1st gen) has a AB7456 chip that is detected as MAX7456 max7456ConfigMutable()->clockConfig = MAX7456_CLOCK_CONFIG_FULL; #endif } diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index a05cd878f0..e961b7c663 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -25,6 +25,7 @@ #define TARGET_BOARD_IDENTIFIER "DYS4" #else #define TARGET_BOARD_IDENTIFIER "OBF4" +#define OMNIBUSF4BASE // For config.c #endif #if defined(LUXF4OSD) From ce824a60d6f8d87c9fb7ef85f22c51c5aa4b0555 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 18 Sep 2017 02:45:39 +1200 Subject: [PATCH 098/138] Fixed mixer for 3D / Dshot. (#4161) --- src/main/blackbox/blackbox.c | 3 - src/main/flight/mixer.c | 125 +++++++++++++++++++---------- src/main/flight/mixer.h | 1 + src/test/unit/blackbox_unittest.cc | 2 +- 4 files changed, 85 insertions(+), 46 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index bbf149ce38..162f2bca6e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -318,9 +318,6 @@ typedef struct blackboxSlowState_s { bool rxFlightChannelsValid; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() -//From mixer.c: -extern float motorOutputHigh, motorOutputLow; - //From rc_controls.c extern boxBitmask_t rcModeActivationMask; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 73c8af4b6d..39f2c71844 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -23,6 +23,7 @@ #include "platform.h" #include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/filter.h" @@ -313,8 +314,11 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; float motorOutputHigh, motorOutputLow; + +static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; +static uint16_t rcCommand3dDeadBandLow; +static uint16_t rcCommand3dDeadBandHigh; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; uint8_t getMotorCount(void) @@ -357,7 +361,9 @@ bool mixerIsOutputSaturated(int axis, float errorRate) // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range -void initEscEndpoints(void) { +void initEscEndpoints(void) +{ + // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet switch (motorConfig()->dev.motorPwmProtocol) { #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: @@ -366,10 +372,11 @@ void initEscEndpoints(void) { case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: disarmMotorOutput = DSHOT_DISARM_COMMAND; - if (feature(FEATURE_3D)) + if (feature(FEATURE_3D)) { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); - else + } else { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); + } motorOutputHigh = DSHOT_MAX_THROTTLE; deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; @@ -377,8 +384,13 @@ void initEscEndpoints(void) { break; #endif default: - disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; - motorOutputLow = motorConfig()->minthrottle; + if (feature(FEATURE_3D)) { + disarmMotorOutput = flight3DConfig()->neutral3d; + motorOutputLow = PWM_RANGE_MIN; + } else { + disarmMotorOutput = motorConfig()->mincommand; + motorOutputLow = motorConfig()->minthrottle; + } motorOutputHigh = motorConfig()->maxthrottle; deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; deadbandMotor3dLow = flight3DConfig()->deadband3d_low; @@ -386,9 +398,13 @@ void initEscEndpoints(void) { break; } - rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); - rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle; - rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck; + + rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle; + + rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN; + rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh; } void mixerInit(mixerMode_e mixerMode) @@ -515,53 +531,82 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -float throttle = 0; -float motorOutputMin, motorOutputMax; -bool mixerInversion = false; -float motorOutputRange; +static float throttle = 0; +static float motorOutputMin; +static float motorRangeMin; +static float motorRangeMax; +static float motorOutputRange; +static int8_t motorOutputMixSign; void calculateThrottleAndCurrentMotorEndpoints(void) { - static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions + static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions float currentThrottleInputRange = 0; if(feature(FEATURE_3D)) { - if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + if (!ARMING_FLAG(ARMED)) { + rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + } - if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { - motorOutputMax = deadbandMotor3dLow; - motorOutputMin = motorOutputLow; - throttlePrevious = rcCommand[THROTTLE]; //3D Mode Throttle Fix #3696 - throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; //3D Mode Throttle Fix #3696 + if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { + // INVERTED + motorRangeMin = motorOutputLow; + motorRangeMax = deadbandMotor3dLow; + if(isMotorProtocolDshot()) { + motorOutputMin = motorOutputLow; + motorOutputRange = deadbandMotor3dLow - motorOutputLow; + } else { + motorOutputMin = deadbandMotor3dLow; + motorOutputRange = motorOutputLow - deadbandMotor3dLow; + } + motorOutputMixSign = -1; + rcThrottlePrevious = rcCommand[THROTTLE]; + throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; currentThrottleInputRange = rcCommandThrottleRange3dLow; - if(isMotorProtocolDshot()) mixerInversion = true; - } else if(rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { - motorOutputMax = motorOutputHigh; + } else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { + // NORMAL + motorRangeMin = deadbandMotor3dHigh; + motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; - throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; + motorOutputMixSign = 1; + rcThrottlePrevious = rcCommand[THROTTLE]; + throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { - motorOutputMax = deadbandMotor3dLow; - motorOutputMin = motorOutputLow; - throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; + } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) { + // INVERTED_TO_DEADBAND + motorRangeMin = motorOutputLow; + motorRangeMax = deadbandMotor3dLow; + if(isMotorProtocolDshot()) { + motorOutputMin = motorOutputLow; + motorOutputRange = deadbandMotor3dLow - motorOutputLow; + } else { + motorOutputMin = deadbandMotor3dLow; + motorOutputRange = motorOutputLow - deadbandMotor3dLow; + } + motorOutputMixSign = -1; + throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dLow; - if(isMotorProtocolDshot()) mixerInversion = true; } else { - motorOutputMax = motorOutputHigh; + // NORMAL_TO_DEADBAND + motorRangeMin = deadbandMotor3dHigh; + motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; + motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; + motorOutputMixSign = 1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dHigh; } } else { throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange; + motorRangeMin = motorOutputLow; + motorRangeMax = motorOutputHigh; motorOutputMin = motorOutputLow; - motorOutputMax = motorOutputHigh; + motorOutputRange = motorOutputHigh - motorOutputLow; } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); - motorOutputRange = motorOutputMax - motorOutputMin; } static void applyFlipOverAfterCrashModeToMotors(void) @@ -597,21 +642,16 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (uint32_t i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)); - - // Dshot works exactly opposite in lower 3D section. - if (mixerInversion) { - motorOutput = motorOutputMin + (motorOutputMax - motorOutput); - } + float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); if (failsafeIsActive()) { if (isMotorProtocolDshot()) { - motorOutput = (motorOutput < motorOutputMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range + motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } - motorOutput = constrain(motorOutput, disarmMotorOutput, motorOutputMax); + motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); } else { - motorOutput = constrain(motorOutput, motorOutputMin, motorOutputMax); + motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } // Motor stop handling @@ -637,6 +677,7 @@ void mixTable(uint8_t vbatPidCompensation) applyFlipOverAfterCrashModeToMotors(); return; } + // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index fd3ef0ceec..e4803a6548 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -105,6 +105,7 @@ PG_DECLARE(motorConfig_t, motorConfig); extern const mixer_t mixers[]; extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern float motorOutputHigh, motorOutputLow; struct rxConfig_s; uint8_t getMotorCount(void); diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index b9b42bf201..3ac929f891 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -365,7 +365,7 @@ int32_t GPS_home[2]; gyro_t gyro; -uint16_t motorOutputHigh, motorOutputLow; +float motorOutputHigh, motorOutputLow; float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct pidProfile_s; struct pidProfile_s *currentPidProfile; From 72a8e91fdd870a9bf96e08f8791c02db5283c7bb Mon Sep 17 00:00:00 2001 From: Bryce Johnson Date: Sun, 17 Sep 2017 16:52:36 -0500 Subject: [PATCH 099/138] Disable small_angle during crashflip mode --- src/main/fc/fc_core.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 22c3ea0084..7c0f14d979 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -190,7 +190,7 @@ void updateArmingStatus(void) unsetArmingDisabled(ARMING_DISABLED_THROTTLE); } - if (!STATE(SMALL_ANGLE)) { + if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { setArmingDisabled(ARMING_DISABLED_ANGLE); } else { unsetArmingDisabled(ARMING_DISABLED_ANGLE); From b20d5e6f80099cae813936f8991984bf660d6259 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 18 Sep 2017 12:44:40 +1200 Subject: [PATCH 100/138] Fixed mixer for non-3D mode. --- src/main/flight/mixer.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 39f2c71844..6c7627c5ca 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -604,6 +604,7 @@ void calculateThrottleAndCurrentMotorEndpoints(void) motorRangeMax = motorOutputHigh; motorOutputMin = motorOutputLow; motorOutputRange = motorOutputHigh - motorOutputLow; + motorOutputMixSign = 1; } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); From 432c9da65937a8598a726ffe716e86c6145eb3f7 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 18 Sep 2017 11:29:17 +0900 Subject: [PATCH 101/138] Fix N-Channel handling --- src/main/drivers/light_ws2811strip_stdperiph.c | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c index 342b3a3f46..bec7ebbac7 100644 --- a/src/main/drivers/light_ws2811strip_stdperiph.c +++ b/src/main/drivers/light_ws2811strip_stdperiph.c @@ -22,6 +22,8 @@ #ifdef LED_STRIP +#include "build/debug.h" + #include "drivers/io.h" #include "drivers/nvic.h" @@ -99,14 +101,17 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) /* PWM1 Mode configuration */ 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; + TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } 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_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); @@ -115,7 +120,11 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) + TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable); + else + TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + TIM_Cmd(timer, ENABLE); dmaInit(timerHardware->dmaIrqHandler, OWNER_LED_STRIP, 0); From 74ba43a13786400295aaf2336acc71a5f06018fe Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 18 Sep 2017 11:31:29 +0900 Subject: [PATCH 102/138] Temporary fix for BLUEJAYF4 and CLRACINGF4 --- src/main/drivers/light_ws2811strip_stdperiph.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/light_ws2811strip_stdperiph.c b/src/main/drivers/light_ws2811strip_stdperiph.c index bec7ebbac7..695a4dcc07 100644 --- a/src/main/drivers/light_ws2811strip_stdperiph.c +++ b/src/main/drivers/light_ws2811strip_stdperiph.c @@ -105,7 +105,11 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; +#ifndef TEMPORARY_FIX_FOR_LED_ON_NCHAN_AND_HAVE_OUTPUT_INVERTED_FIX_ME_FOR_3_3 + TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; +#else TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; +#endif } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; @@ -120,10 +124,11 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable); - else + } else { TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + } TIM_Cmd(timer, ENABLE); From 860ec23fbe1916d96e458edca498af26cc304fc2 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 19 Sep 2017 00:59:53 +1200 Subject: [PATCH 103/138] Fixed check for halfduplex mode in CRSF. --- src/main/rx/crsf.c | 2 +- src/test/unit/rx_crsf_unittest.cc | 4 ++++ src/test/unit/telemetry_crsf_msp_unittest.cc | 1 + src/test/unit/telemetry_crsf_unittest.cc | 2 ++ 4 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index e06c6dc8d1..1c598ca537 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -233,7 +233,7 @@ void crsfRxSendTelemetryData(void) if (telemetryBufLen > 0) { // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) // and that there is time to send the telemetry frame before the next RX frame arrives - if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) { + if (rxConfig()->halfDuplex) { const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt; if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 09fa725b5f..0b41b35325 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -26,6 +26,8 @@ extern "C" { #include "build/debug.h" + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" #include "common/crc.h" #include "common/utils.h" @@ -47,6 +49,8 @@ extern "C" { extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; uint32_t dummyTimeUs; + + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); } #include "unittest_macros.h" diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 8ef8099531..2913bc6867 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -77,6 +77,7 @@ extern "C" { PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); extern bool crsfFrameDone; extern crsfFrame_t crsfFrame; diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 2744947275..50f84bf393 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -49,6 +49,7 @@ extern "C" { #include "io/gps.h" #include "io/serial.h" + #include "rx/rx.h" #include "rx/crsf.h" #include "sensors/battery.h" @@ -68,6 +69,7 @@ extern "C" { PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); } #include "unittest_macros.h" From 29bbaadceb34138a1709a622fdcf86426a393e43 Mon Sep 17 00:00:00 2001 From: dangeljs Date: Mon, 18 Sep 2017 18:30:41 -0400 Subject: [PATCH 104/138] Updating CC3D target file to allow for Parallel PWM to work properly with ESC motors on v2.1 and greater. --- src/main/target/CC3D/target.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index 86ea41bebc..8702ba8021 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -26,17 +26,19 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM4, CH1, PB6, TIM_USE_PWM, 0), // S1_IN/ +// DEF_TIM(TIM4, CH1, PB6, TIM_USE_PWM, 0), // S1_IN/ <--Moved to allow Parallel PWM with proper ESC DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // S2_IN - SoftSerial TX - GPIO_PartialRemap_TIM3 / Sonar trigger DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), // S4_IN - Current DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM, 0), // S5_IN - Vbattery - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM | TIM_USE_PPM, 0), // S6_IN - PPM IN + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), // S6_IN - PPM IN + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 1), // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // S1_OUT DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // S2_OUT DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 1), // S3_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // S4_OUT - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 1), // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0), // S1_IN/ + // DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 1), // S5_OUT <--Moved to allow Parallel PWM with proper ESC DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1) // S6_OUT }; From fd29a8eff08a268372a7ce9a9d29ca57004d92ef Mon Sep 17 00:00:00 2001 From: dangeljs Date: Mon, 18 Sep 2017 20:24:25 -0400 Subject: [PATCH 105/138] Reinstall the PPM pin on Timer2 CH2. Issue #2177 --- src/main/target/CC3D/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index 8702ba8021..386bd33df4 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -31,7 +31,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // S3_IN - SoftSerial RX / Sonar echo / RSSI ADC DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), // S4_IN - Current DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM, 0), // S5_IN - Vbattery - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), // S6_IN - PPM IN + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM| TIM_USE_PPM, 0), // S6_IN - PPM IN DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 1), // S5_OUT - GPIO_PartialRemap_TIM3 - LED Strip DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1), // S1_OUT DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1), // S2_OUT From f3f7029e851c60dad6179911e42ad4bcb1cdd19d Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 19 Sep 2017 10:28:24 +0900 Subject: [PATCH 106/138] Do proper IOConfigGPIOAF in ppmRxInit and pwmRxInit Regression after #3991 --- src/main/drivers/rx_pwm.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 044b8a24b2..f3c05084d7 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -385,12 +385,9 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IOInit(io, OWNER_PWMINPUT, RESOURCE_INDEX(channel)); #ifdef STM32F1 IOConfigGPIO(io, IOCFG_IPD); -#elif defined(STM32F7) - IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #else - IOConfigGPIO(io, IOCFG_AF_PP); + IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #endif - timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ); timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback); timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback); @@ -442,10 +439,8 @@ void ppmRxInit(const ppmConfig_t *ppmConfig) IOInit(io, OWNER_PPMINPUT, 0); #ifdef STM32F1 IOConfigGPIO(io, IOCFG_IPD); -#elif defined(STM32F7) - IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #else - IOConfigGPIO(io, IOCFG_AF_PP); + IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); #endif timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ); From 4e867c00bcb9a70763da3fc939ea4f4339da9b2a Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 19 Sep 2017 16:03:07 +0900 Subject: [PATCH 107/138] Timer mapping rework - UART backdoors are added for maximum flexibility. - PPM is assigned to TIM10_CH1 to avoid colliding with LED strip. - Motors 5 and 6 (S5_OUT and S6_OUT) removed, as they are not wired. - Backdoor on UART6 are removed, as timers on these pins conflict with motors (TIM3 and TIM8). - target.h has been modified to accommodate these changes. --- src/main/target/BETAFLIGHTF4/target.c | 19 +++++++++++++------ src/main/target/BETAFLIGHTF4/target.h | 22 ++++++++++------------ 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index 7d30bf3bc7..b9d1c60759 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -25,15 +25,22 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM - DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM + // Motors DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S1_OUT D1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S2_OUT D1_ST2 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S3_OUT D1_ST6 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S5_OUT - DEF_TIM(TIM3, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT D1_ST2 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 TX Softserial Smartport - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 RX - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED + + // LED strip + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // D1_ST0 + + // UART Backdoors + DEF_TIM(TIM1, CH2, PA9, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // TX1 Bidir + DEF_TIM(TIM1, CH3, PA10, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // RX1 Bidir + DEF_TIM(TIM5, CH3, PA2, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // TX2 TX only + DEF_TIM(TIM9, CH2, PA3, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // RX2 RX only + DEF_TIM(TIM2, CH3, PB10, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // TX3 Bidir + DEF_TIM(TIM2, CH4, PB11, TIM_USE_NONE, TIMER_OUTPUT_STANDARD, 0), // RX3 Bidir }; diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 10b5168af8..c383ae0aaf 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -27,8 +27,8 @@ // Leave beeper here but with none as io - so disabled unless mapped. #define BEEPER PB4 -// PC0 used as inverter select GPIO -#define INVERTER_PIN_UART6 PC13 +// PC13 used as inverter select GPIO for UART2 +#define INVERTER_PIN_UART2 PC13 #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -90,10 +90,9 @@ #define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 -#define SOFTSERIAL1_RX_PIN PC7 -#define SOFTSERIAL1_TX_PIN PC6 +#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 6 //VCP, USART1, USART2, USART3, USART6 +#define SERIAL_PORT_COUNT 7 //VCP, USART1, USART2, USART3, USART6, SOFTSERIAL1, SOFTSERIAL2 #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_PIN PB8 // (Hardware=0, PPM) @@ -135,12 +134,11 @@ #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 TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTB (0xffff & ~(BIT(2))) +#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14))) +#define TARGET_IO_PORTD BIT(2) +#define USABLE_TIMER_CHANNEL_COUNT 12 -#define USABLE_TIMER_CHANNEL_COUNT 10 - -#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9) | TIM_N(10) ) From 298082375d24a1557f0fd761843720bbc732b8e6 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 19 Sep 2017 18:52:21 +0900 Subject: [PATCH 108/138] Add I2C2 on TX3/RX3 --- src/main/target/BETAFLIGHTF4/target.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index c383ae0aaf..a17bb56086 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -113,6 +113,11 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL NONE // PB10, UART3_TX +#define I2C2_SDA NONE // PB11, UART3_RX +#define I2C_DEVICE (I2CDEV_2) #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 From 8ff2c9ba957447e499b08ccd2dfc545b37867c9c Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 18 Sep 2017 20:01:28 +1200 Subject: [PATCH 109/138] Renamed 'DSHOT_CMD_BEEPx' to 'DSHOT_CMD_BEACONx', made beacon tone configurable. --- src/main/drivers/pwm_output.h | 10 +++++----- src/main/fc/config.c | 6 ------ src/main/fc/settings.c | 2 +- src/main/io/beeper.c | 9 +++++++-- src/main/io/beeper.h | 2 +- 5 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index c95590226f..9767a13726 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -39,11 +39,11 @@ typedef enum { DSHOT_CMD_MOTOR_STOP = 0, - DSHOT_CMD_BEEP1, - DSHOT_CMD_BEEP2, - DSHOT_CMD_BEEP3, - DSHOT_CMD_BEEP4, - DSHOT_CMD_BEEP5, + DSHOT_CMD_BEACON1, + DSHOT_CMD_BEACON2, + DSHOT_CMD_BEACON3, + DSHOT_CMD_BEACON4, + DSHOT_CMD_BEACON5, DSHOT_CMD_ESC_INFO, // V2 includes settings DSHOT_CMD_SPIN_DIRECTION_1, DSHOT_CMD_SPIN_DIRECTION_2, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a86d7183b8..c8664976cc 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -156,12 +156,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, ); #endif -#ifdef BEEPER -PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); -PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, - .dshotForward = true -); -#endif #ifdef USE_ADC PG_REGISTER_WITH_RESET_FN(adcConfig_t, adcConfig, PG_ADC_CONFIG, 0); #endif diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 9fbdc1073e..25c660b65d 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -467,7 +467,7 @@ const clivalue_t valueTable[] = { // PG_BEEPER_CONFIG #ifdef USE_DSHOT - { "beeper_dshot", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotForward) }, + { "beeper_dshot_beacon_tone", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, DSHOT_CMD_BEACON5 }, PG_BEEPER_CONFIG, offsetof(beeperConfig_t, dshotBeaconTone) }, #endif #endif diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 5ce73ff074..9c03769420 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -84,6 +84,11 @@ PG_RESET_TEMPLATE(beeperDevConfig_t, beeperDevConfig, #define BEEPER_COMMAND_STOP 0xFF #ifdef BEEPER +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); +PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, + .dshotBeaconTone = 0 +); + /* Beeper Sound Sequences: (Square wave generation) * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from * start when 0xFF stops the sound when it's completed. @@ -363,11 +368,11 @@ void beeperUpdate(timeUs_t currentTimeUs) } #ifdef USE_DSHOT - if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { + if (!areMotorsRunning() && beeperConfig()->dshotBeaconTone && (beeperConfig()->dshotBeaconTone <= DSHOT_CMD_BEACON5) && currentBeeperEntry->mode == BEEPER_RX_SET) { pwmDisableMotors(); delay(1); - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3); + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone); pwmEnableMotors(); } diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index b4d8de0211..997539b80e 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -51,7 +51,7 @@ typedef enum { typedef struct beeperConfig_s { uint32_t beeper_off_flags; uint32_t preferred_beeper_off_flags; - bool dshotForward; + uint8_t dshotBeaconTone; } beeperConfig_t; #ifdef BEEPER From 51aa8e8177a9cfc3789eb4953381279e27a73782 Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 19 Sep 2017 23:15:25 +0900 Subject: [PATCH 110/138] Fix typo for VTX_RTC6705_OPTIONAL and add vtxCommonDeviceRegistered --- src/main/drivers/vtx_common.c | 5 +++++ src/main/drivers/vtx_common.h | 1 + src/main/fc/fc_init.c | 2 +- 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index fff7fcab6b..b58121487d 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -42,6 +42,11 @@ void vtxCommonRegisterDevice(vtxDevice_t *pDevice) vtxDevice = pDevice; } +bool vtxCommonDeviceRegistered(void) +{ + return vtxDevice; +} + void vtxCommonProcess(uint32_t currentTimeUs) { if (!vtxDevice) diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 000859ccab..fc74f130d8 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -78,6 +78,7 @@ typedef struct vtxVTable_s { void vtxCommonInit(void); void vtxCommonRegisterDevice(vtxDevice_t *pDevice); +bool vtxCommonDeviceRegistered(void); // VTable functions void vtxCommonProcess(uint32_t currentTimeUs); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 27c4c56c57..9edd9cc452 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -677,7 +677,7 @@ void init(void) #endif #ifdef VTX_RTC6705 -#ifdef VTX_RTC6705OPTIONAL +#ifdef VTX_RTC6705_OPTIONAL if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured. #endif { From e29c187c7a445f05654a1867d61eada5e9bb13bd Mon Sep 17 00:00:00 2001 From: jflyper Date: Tue, 19 Sep 2017 23:31:15 +0900 Subject: [PATCH 111/138] Fix typo. --- src/main/target/SPRACINGF4EVO/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index df20a803c4..cf944f207f 100644 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -24,7 +24,7 @@ #define SPRACINGF4EVO_REV 2 #endif -#define USBD_PRODUCT_STRING "SP Racing F4 NEO" +#define USBD_PRODUCT_STRING "SP Racing F4 EVO" #define LED0_PIN PA0 From 4500488c9465adfac1354ba07f36a279b6e4f0ee Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 20 Sep 2017 11:05:24 +0900 Subject: [PATCH 112/138] Avoid timer conflict on TIM4 between PPM and LED strip PPM (PB8) can be assigned to TIM10_CH1. --- src/main/target/OMNIBUSF4/target.c | 2 +- src/main/target/OMNIBUSF4/target.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 27bbf82d88..9374a07e69 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -27,7 +27,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { #if defined(OMNIBUSF4SD) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM DEF_TIM(TIM4, CH4, PB9, TIM_USE_PWM, TIMER_OUTPUT_NONE, 0), // S2_IN #else DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index f2bbd61996..6670b079ab 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -234,7 +234,8 @@ #ifdef OMNIBUSF4SD #define USABLE_TIMER_CHANNEL_COUNT 15 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(10) | TIM_N(12) | TIM_N(8) | TIM_N(9)) #else #define USABLE_TIMER_CHANNEL_COUNT 14 -#endif #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#endif From 503b8dbbfee0fc2381cf98487ffec9a606ac8c95 Mon Sep 17 00:00:00 2001 From: jflyper Date: Wed, 20 Sep 2017 12:11:23 +0900 Subject: [PATCH 113/138] Issue proper IOConfigGPIOAF for non-F1 targets --- src/main/drivers/camera_control.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c index e1daf12994..82ef9ef0f7 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/camera_control.c @@ -108,10 +108,10 @@ void cameraControlInit() return; } - #ifdef USE_HAL_DRIVER - IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction); + #ifdef STM32F1 + IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP); #else - IOConfigGPIO(cameraControlRuntime.io, IOCFG_AF_PP); + IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction); #endif pwmOutConfig(&cameraControlRuntime.channel, timerHardware, CAMERA_CONTROL_TIMER_HZ, CAMERA_CONTROL_PWM_RESOLUTION, 0, 0); From b48286fee2fa497a46be4cb6eee3b2f6af7ca9e0 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 16 Sep 2017 17:47:19 +0200 Subject: [PATCH 114/138] FortiniF4 add camera control pin --- src/main/target/FF_FORTINIF4/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 5c2db86ac7..c23c4d010d 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -32,6 +32,10 @@ #define BEEPER_INVERTED /*---------------------------------*/ +/*----------CAMERA CONTROL---------*/ +#define CAMERA_CONTROL_PIN PB7 +/*---------------------------------*/ + /*------------SENSORS--------------*/ // MPU interrupt #define USE_EXTI From c5ce6d6d35737e0849ff8af67e4d7475d756cdb4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 20 Sep 2017 10:48:18 +0100 Subject: [PATCH 115/138] Added lightweight string handling from iNav --- src/main/common/string_light.c | 74 ++++++++++++++++++++++++++++++++++ src/main/common/string_light.h | 28 +++++++++++++ 2 files changed, 102 insertions(+) create mode 100755 src/main/common/string_light.c create mode 100755 src/main/common/string_light.h diff --git a/src/main/common/string_light.c b/src/main/common/string_light.c new file mode 100755 index 0000000000..1546f733c4 --- /dev/null +++ b/src/main/common/string_light.c @@ -0,0 +1,74 @@ +/* + * 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 "string_light.h" +#include "typeconversion.h" + +int sl_isalnum(int c) +{ + return sl_isdigit(c) || sl_isupper(c) || sl_islower(c); +} + +int sl_isdigit(int c) +{ + return (c >= '0' && c <= '9'); +} + +int sl_isupper(int c) +{ + return (c >= 'A' && c <= 'Z'); +} + +int sl_islower(int c) +{ + return (c >= 'a' && c <= 'z'); +} + +int sl_tolower(int c) +{ + return sl_isupper(c) ? (c) - 'A' + 'a' : c; +} + +int sl_toupper(int c) +{ + return sl_islower(c) ? (c) - 'a' + 'A' : c; +} + +int sl_strcasecmp(const char * s1, const char * s2) +{ + return sl_strncasecmp(s1, s2, INT_MAX); +} + +int sl_strncasecmp(const char * s1, const char * s2, int n) +{ + const unsigned char * ucs1 = (const unsigned char *) s1; + const unsigned char * ucs2 = (const unsigned char *) s2; + + int d = 0; + + for ( ; n != 0; n--) { + const int c1 = sl_tolower(*ucs1++); + const int c2 = sl_tolower(*ucs2++); + if (((d = c1 - c2) != 0) || (c2 == '\0')) { + break; + } + } + + return d; +} diff --git a/src/main/common/string_light.h b/src/main/common/string_light.h new file mode 100755 index 0000000000..005a6c6b12 --- /dev/null +++ b/src/main/common/string_light.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 + +int sl_isalnum(int c); +int sl_isdigit(int c); +int sl_isupper(int c); +int sl_islower(int c); +int sl_tolower(int c); +int sl_toupper(int c); + +int sl_strcasecmp(const char * s1, const char * s2); +int sl_strncasecmp(const char * s1, const char * s2, int n); From 287a45659d88f7bc5f97cb265eb47a5d811cc61e Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 20 Sep 2017 10:43:51 +0100 Subject: [PATCH 116/138] CRSF code tidy --- src/main/fc/fc_core.c | 4 +++- src/main/fc/fc_rc.c | 6 ++++-- src/main/rx/crsf.c | 33 +++++++++++++++---------------- src/main/rx/crsf.h | 2 +- src/main/telemetry/crsf.c | 18 +++++++++-------- src/main/telemetry/crsf.h | 4 ++-- src/main/telemetry/msp_shared.c | 22 ++++++++++----------- src/main/telemetry/msp_shared.h | 2 +- src/test/unit/rx_crsf_unittest.cc | 4 ++-- 9 files changed, 50 insertions(+), 45 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 7c0f14d979..eb179c9893 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -368,7 +368,9 @@ void updateMagHold(void) } #endif - +/* + * processRx called from taskUpdateRxMain + */ void processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index f0ac7e669b..94299c0ebe 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -134,7 +134,8 @@ static void calculateSetpointRate(int axis) setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) } -static void scaleRcCommandToFpvCamAngle(void) { +static void scaleRcCommandToFpvCamAngle(void) +{ //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed static uint8_t lastFpvCamAngleDegrees = 0; static float cosFactor = 1.0; @@ -155,7 +156,8 @@ static void scaleRcCommandToFpvCamAngle(void) { #define THROTTLE_BUFFER_MAX 20 #define THROTTLE_DELTA_MS 100 - static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) { + static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) + { static int index; static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; const int rxRefreshRateMs = rxRefreshRate / 1000; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 1c598ca537..1e82f0a8d7 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -59,7 +59,7 @@ STATIC_UNIT_TESTED crsfFrame_t crsfFrame; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; -static uint32_t crsfFrameStartAt = 0; +static uint32_t crsfFrameStartAtUs = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; @@ -75,13 +75,13 @@ static uint8_t telemetryBufLen = 0; * 1 Stop bit * Big endian * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte - * Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes - * A 36 byte frame can be transmitted in 771 microseconds. + * Max frame size is 64 bytes + * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds. * - * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds + * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds * * Every frame has the structure: - * < Type> < CRC> + * * * Device address: (uint8_t) * Frame length: length in bytes including Type (uint8_t) @@ -117,20 +117,20 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t; STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) { static uint8_t crsfFramePosition = 0; - const uint32_t now = micros(); + const uint32_t currentTimeUs = micros(); #ifdef DEBUG_CRSF_PACKETS - debug[2] = now - crsfFrameStartAt; + debug[2] = currentTimeUs - crsfFrameStartAtUs; #endif - if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) { + if (currentTimeUs > crsfFrameStartAtUs + CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. crsfFramePosition = 0; } if (crsfFramePosition == 0) { - crsfFrameStartAt = now; + crsfFrameStartAtUs = currentTimeUs; } // assume frame is 5 bytes long until we have received the frame length // full frame length includes the length of the address and framelength fields @@ -167,7 +167,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) } crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; // unpack the RC channels - const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; + const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; crsfChannelData[0] = rcChannels->chan0; crsfChannelData[1] = rcChannels->chan1; crsfChannelData[2] = rcChannels->chan2; @@ -188,15 +188,15 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) } else { if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) { // TODO: CRC CHECK - scheduleDeviceInfoResponse(); + crsfScheduleDeviceInfoResponse(); return RX_FRAME_COMPLETE; #if defined(USE_MSP_OVER_TELEMETRY) } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) { // TODO: CRC CHECK uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE; - if(handleMspFrame(frameStart, frameEnd)) { - scheduleMspResponse(); + if (handleMspFrame(frameStart, frameEnd)) { + crsfScheduleMspResponse(); } return RX_FRAME_COMPLETE; #endif @@ -234,9 +234,9 @@ void crsfRxSendTelemetryData(void) // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) // and that there is time to send the telemetry frame before the next RX frame arrives if (rxConfig()->halfDuplex) { - const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt; - if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || - (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { + const uint32_t timeSinceStartOfFrameUs = micros() - crsfFrameStartAtUs; + if ((timeSinceStartOfFrameUs < CRSF_TIME_NEEDED_PER_FRAME_US) || + (timeSinceStartOfFrameUs > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { return; } } @@ -247,7 +247,6 @@ void crsfRxSendTelemetryData(void) bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { - for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408; } diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 0b547cbc67..8342a29917 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -69,7 +69,7 @@ enum { CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE }; -#define CRSF_PAYLOAD_SIZE_MAX 60 // Size confirmed by Remo +#define CRSF_PAYLOAD_SIZE_MAX 60 #define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) typedef struct crsfFrameDef_s { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d7d18bf692..64e054afd4 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -63,9 +63,6 @@ static bool crsfTelemetryEnabled; static bool deviceInfoReplyPending; -#if defined(USE_MSP_OVER_TELEMETRY) -static bool mspReplyPending; -#endif static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; static void crsfInitializeFrame(sbuf_t *dst) @@ -232,10 +229,6 @@ void crsfFrameFlightMode(sbuf_t *dst) *lengthPtr = sbufPtr(dst) - lengthPtr; } -void scheduleDeviceInfoResponse() { - deviceInfoReplyPending = true; -} - /* 0x29 Device Info Payload: @@ -283,7 +276,11 @@ static uint8_t crsfScheduleCount; static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) -void scheduleMspResponse() { + +static bool mspReplyPending; + +void crsfScheduleMspResponse(void) +{ mspReplyPending = true; } @@ -336,6 +333,11 @@ static void processCrsf(void) crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } +void crsfScheduleDeviceInfoResponse(void) +{ + deviceInfoReplyPending = true; +} + void initCrsfTelemetry(void) { // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index b183d2927b..3ccfc76cac 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -24,7 +24,7 @@ void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); -void scheduleDeviceInfoResponse(); -void scheduleMspResponse(); +void crsfScheduleDeviceInfoResponse(void); +void crsfScheduleMspResponse(void); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 074b9f92b0..d80ec2a168 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -46,7 +46,8 @@ static mspTxBuffer_t mspTxBuffer; static mspPacket_t mspRxPacket; static mspPacket_t mspTxPacket; -void initSharedMsp() { +void initSharedMsp(void) +{ mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; mspPackage.requestPacket = &mspRxPacket; mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer; @@ -58,7 +59,7 @@ void initSharedMsp() { mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; } -static void processMspPacket() +static void processMspPacket(void) { mspPackage.responsePacket->cmd = 0; mspPackage.responsePacket->result = 0; @@ -102,9 +103,9 @@ bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) mspPacket_t *packet = mspPackage.requestPacket; sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd); sbuf_t *rxBuf = &mspPackage.requestPacket->buf; - uint8_t header = sbufReadU8(frameBuf); - uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; - uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; + const uint8_t header = sbufReadU8(frameBuf); + const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; + const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; if (version != TELEMETRY_MSP_VERSION) { sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); @@ -131,8 +132,8 @@ bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd) return false; } - uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); - uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); + const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); + const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); uint8_t payload[frameBytesRemaining]; if (bufferBytesRemaining >= frameBytesRemaining) { @@ -185,14 +186,13 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) uint8_t size = sbufBytesRemaining(txBuf); sbufWriteU8(payloadBuf, size); - } - else { + } else { // header sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); } - uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); - uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); + const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); + const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); uint8_t frame[payloadBytesRemaining]; if (bufferBytesRemaining >= payloadBytesRemaining) { diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index 315c47a615..85a3e2c703 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -24,6 +24,6 @@ typedef union mspTxBuffer_u { uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE]; } mspTxBuffer_t; -void initSharedMsp(); +void initSharedMsp(void); bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 0b41b35325..1aa2e89642 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -286,7 +286,7 @@ serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, seria serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; -void scheduleDeviceInfoResponse(void) {}; -void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; +void crsfScheduleDeviceInfoResponse(void) {}; +void crsfScheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; bool handleMspFrame(uint8_t *, uint8_t *) { return false; } } From bc799fd99ff5916f9679db930470e177bc95cad9 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 21 Sep 2017 10:12:45 +0900 Subject: [PATCH 117/138] Remove space from arming disabled flag print name for readability --- src/main/fc/runtime_config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 66444de380..0094679329 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -31,8 +31,8 @@ static uint32_t enabledSensors = 0; #if defined(OSD) || !defined(MINIMAL_CLI) const char *armingDisableFlagNames[]= { - "NOGYRO", "FAILSAFE", "RX LOSS", "BAD RX", "BOXFAILSAFE", - "THROTTLE", "ANGLE", "BOOT GRACE", "NO PREARM", "ARM SWITCH", + "NOGYRO", "FAILSAFE", "RXLOSS", "BADRX", "BOXFAILSAFE", + "THROTTLE", "ANGLE", "BOOTGRACE", "NOPREARM", "ARMSWITCH", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST" }; #endif From 39613d15e59b41dc896310141c5dbff85192def4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 21 Sep 2017 05:20:39 +0100 Subject: [PATCH 118/138] Code tidy of fc_rc.c --- src/main/fc/fc_rc.c | 34 +++++++++++++++++++++------------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 94299c0ebe..3d4439be2b 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -156,24 +156,27 @@ static void scaleRcCommandToFpvCamAngle(void) #define THROTTLE_BUFFER_MAX 20 #define THROTTLE_DELTA_MS 100 - static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) - { +static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) +{ static int index; static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; + const int rxRefreshRateMs = rxRefreshRate / 1000; const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); const int16_t throttleVelocityThreshold = (feature(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; - if (index >= indexMax) + if (index >= indexMax) { index = 0; + } const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - if (ABS(rcCommandSpeed) > throttleVelocityThreshold) + if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); - else + } else { pidSetItermAccelerator(1.0f); + } } void processRcCommand(void) @@ -182,10 +185,6 @@ void processRcCommand(void) static float rcStepSize[4] = { 0, 0, 0, 0 }; static int16_t rcInterpolationStepCount; static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" - uint16_t rxRefreshRate; - bool readyToCalculateRate = false; - uint8_t readyToCalculateRateAxisCnt = 0; if (isRXDataNew) { currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); @@ -194,6 +193,11 @@ void processRcCommand(void) } } + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" + uint16_t rxRefreshRate; + bool readyToCalculateRate = false; + uint8_t readyToCalculateRateAxisCnt = 0; + if (rxConfig()->rcInterpolation) { // Set RC refresh rate for sampling and channels to filter switch (rxConfig()->rcInterpolation) { @@ -240,19 +244,22 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - if (isRXDataNew) + if (isRXDataNew) { readyToCalculateRateAxisCnt = FD_YAW; + } - for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) + for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) { calculateSetpointRate(axis); + } if (debugMode == DEBUG_RC_INTERPOLATION) { debug[2] = rcInterpolationStepCount; debug[3] = setpointRate[0]; } // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) - if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) + if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { scaleRcCommandToFpvCamAngle(); + } isRXDataNew = false; } @@ -330,7 +337,8 @@ void updateRcCommands(void) } } -void resetYawAxis(void) { +void resetYawAxis(void) +{ rcCommand[YAW] = 0; setpointRate[YAW] = 0; } From 91c761a45118014a668f5de080229c18c5c3d2e6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 21 Sep 2017 11:41:49 +0100 Subject: [PATCH 119/138] Hott telemetry tidy --- src/main/telemetry/hott.c | 91 +++++++++++++++++++-------------------- 1 file changed, 45 insertions(+), 46 deletions(-) diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 4ead0d2c0b..27d84c223a 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -58,6 +58,7 @@ #include "platform.h" + #ifdef TELEMETRY #include "build/build_config.h" @@ -65,29 +66,24 @@ #include "common/axis.h" #include "common/time.h" -#include "common/utils.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" +#include "drivers/serial.h" #include "drivers/time.h" -#include "io/serial.h" - #include "fc/runtime_config.h" -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/barometer.h" - +#include "flight/altitude.h" #include "flight/pid.h" #include "flight/navigation.h" + #include "io/gps.h" -#include "flight/altitude.h" +#include "sensors/battery.h" +#include "sensors/barometer.h" +#include "sensors/sensors.h" -#include "telemetry/telemetry.h" #include "telemetry/hott.h" +#include "telemetry/telemetry.h" //#define HOTT_DEBUG @@ -229,16 +225,13 @@ static bool shouldTriggerBatteryAlarmNow(void) static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) { - batteryState_e batteryState; - if (shouldTriggerBatteryAlarmNow()) { lastHottAlarmSoundTime = millis(); - batteryState = getBatteryState(); + const batteryState_e batteryState = getBatteryState(); if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { hottEAMMessage->warning_beeps = 0x10; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1; - } - else { + } else { hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE; hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE; } @@ -257,14 +250,14 @@ static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t amp = getAmperage() / 10; + const int32_t amp = getAmperage() / 10; hottEAMMessage->current_L = amp & 0xFF; hottEAMMessage->current_H = amp >> 8; } static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t mAh = getMAhDrawn() / 10; + const int32_t mAh = getMAhDrawn() / 10; hottEAMMessage->batt_cap_L = mAh & 0xFF; hottEAMMessage->batt_cap_H = mAh >> 8; } @@ -279,7 +272,7 @@ static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t vario = getEstimatedVario(); + const int32_t vario = getEstimatedVario(); hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF; hottEAMMessage->climbrate_H = (30000 + vario) >> 8; hottEAMMessage->climbrate3s = 120 + (vario / 100); @@ -327,7 +320,8 @@ static void flushHottRxBuffer(void) } } -static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e mode) { +static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e mode) +{ closeSerialPort(hottPort); portOptions_e portOptions = telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED; @@ -339,28 +333,34 @@ static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, portOptions); } -static bool hottIsUsingHardwareUART(void) { +static bool hottIsUsingHardwareUART(void) +{ return !(portConfig->identifier == SERIAL_PORT_SOFTSERIAL1 || portConfig->identifier == SERIAL_PORT_SOFTSERIAL2); } -static void hottConfigurePortForTX(void) { +static void hottConfigurePortForTX(void) +{ // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences - if (hottIsUsingHardwareUART()) + if (hottIsUsingHardwareUART()) { workAroundForHottTelemetryOnUsart(hottPort, MODE_TX); - else + } else { serialSetMode(hottPort, MODE_TX); + } } -static void hottConfigurePortForRX(void) { +static void hottConfigurePortForRX(void) +{ // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences - if (hottIsUsingHardwareUART()) + if (hottIsUsingHardwareUART()) { workAroundForHottTelemetryOnUsart(hottPort, MODE_RX); - else + } else { serialSetMode(hottPort, MODE_RX); + } flushHottRxBuffer(); } -static void hottReconfigurePort(void) { +static void hottReconfigurePort(void) +{ if (!hottIsSending) { hottIsSending = true; hottMsgCrc = 0; @@ -426,7 +426,8 @@ static void hottPrepareMessages(void) { #endif } -static void processBinaryModeRequest(uint8_t address) { +static void processBinaryModeRequest(uint8_t address) +{ #ifdef HOTT_DEBUG static uint8_t hottBinaryRequests = 0; @@ -436,24 +437,23 @@ static void processBinaryModeRequest(uint8_t address) { switch (address) { #ifdef GPS - case 0x8A: + case 0x8A: #ifdef HOTT_DEBUG - hottGPSRequests++; + hottGPSRequests++; #endif - if (sensors(SENSOR_GPS)) { - hottSendGPSResponse(); - } - break; + if (sensors(SENSOR_GPS)) { + hottSendGPSResponse(); + } + break; #endif - case 0x8E: + case 0x8E: #ifdef HOTT_DEBUG - hottEAMRequests++; + hottEAMRequests++; #endif - hottSendEAMResponse(); - break; + hottSendEAMResponse(); + break; } - #ifdef HOTT_DEBUG hottBinaryRequests++; debug[0] = hottBinaryRequests; @@ -462,14 +462,13 @@ static void processBinaryModeRequest(uint8_t address) { #endif debug[2] = hottEAMRequests; #endif - } static void hottCheckSerialData(uint32_t currentMicros) { static bool lookingForRequest = true; - uint8_t bytesWaiting = serialRxBytesWaiting(hottPort); + const uint8_t bytesWaiting = serialRxBytesWaiting(hottPort); if (bytesWaiting <= 1) { return; @@ -494,8 +493,8 @@ static void hottCheckSerialData(uint32_t currentMicros) lookingForRequest = true; } - uint8_t requestId = serialRead(hottPort); - uint8_t address = serialRead(hottPort); + const uint8_t requestId = serialRead(hottPort); + const uint8_t address = serialRead(hottPort); if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) { /* @@ -538,7 +537,7 @@ static inline bool shouldCheckForHoTTRequest() void checkHoTTTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing); + const bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing); if (newTelemetryEnabledValue == hottTelemetryEnabled) { return; From 8b0d848ce91be0f7f8a9ac76a39e0590fb2f9989 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 21 Sep 2017 11:14:21 +1200 Subject: [PATCH 120/138] Fixed and cleaned up the conversion between Dshot and external PWM values. --- src/main/flight/mixer.c | 67 ++++++++++++++++++++++++----------------- 1 file changed, 40 insertions(+), 27 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6c7627c5ca..802b3a533e 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -104,12 +104,7 @@ 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 -#define EXTERNAL_CONVERSION_MIN_VALUE 1000 -#define EXTERNAL_CONVERSION_MAX_VALUE 2000 -#define EXTERNAL_CONVERSION_3D_MID_VALUE 1500 +#define PWM_RANGE_MID 1500 #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight @@ -743,44 +738,62 @@ void mixTable(uint8_t vbatPidCompensation) float convertExternalToMotor(uint16_t externalValue) { - uint16_t motorValue = externalValue; + uint16_t motorValue; + switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT - if (isMotorProtocolDshot()) { - // Add 1 to the value, otherwise throttle tops out at 2046 - motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR + 1, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); + case true: + externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX); if (feature(FEATURE_3D)) { - if (externalValue == EXTERNAL_CONVERSION_3D_MID_VALUE) { + if (externalValue == PWM_RANGE_MID) { motorValue = DSHOT_DISARM_COMMAND; - } else if (motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { - // Add 1 to the value, otherwise throttle tops out at 2046 - motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) + 1; + } else if (externalValue < PWM_RANGE_MID) { + motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE); + } else { + motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); } + } else { + motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_DISARM_COMMAND : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); } - } + + break; + case false: #endif + default: + motorValue = externalValue; + + break; + } return (float)motorValue; } uint16_t convertMotorToExternal(float motorValue) { - uint16_t externalValue = lrintf(motorValue); + uint16_t externalValue; + switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT - if (isMotorProtocolDshot()) { - if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { - // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor() - motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) - 1; + case true: + if (feature(FEATURE_3D)) { + if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) { + externalValue = PWM_RANGE_MID; + } else if (motorValue <= DSHOT_3D_DEADBAND_LOW) { + externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW, PWM_RANGE_MID - 1, PWM_RANGE_MIN); + } else { + externalValue = scaleRange(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX); + } + } else { + externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX); } - // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor() - externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain(((motorValue - 1)/ EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE); - - if (feature(FEATURE_3D) && motorValue == DSHOT_DISARM_COMMAND) { - externalValue = EXTERNAL_CONVERSION_3D_MID_VALUE; - } - } + break; + case false: #endif + default: + externalValue = motorValue; + + break; + } return externalValue; } From 1016ec146c057aaeeeafcdca769677dc986bb614 Mon Sep 17 00:00:00 2001 From: Bryce Johnson Date: Thu, 21 Sep 2017 11:19:34 -0500 Subject: [PATCH 121/138] Remove pid sum limit so crashflip gets 100% throttle --- src/main/flight/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6c7627c5ca..28103e47f5 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -616,9 +616,9 @@ static void applyFlipOverAfterCrashModeToMotors(void) for (int i = 0; i < motorCount; i++) { if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { - motorMix[i] = getRcDeflection(FD_ROLL) * pidSumLimit * currentMixer[i].roll * (-1); + motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * (-1); } else { - motorMix[i] = getRcDeflection(FD_PITCH) * pidSumLimit * currentMixer[i].pitch * (-1); + motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * (-1); } } // Apply the mix to motor endpoints From 79651ec6c83f96f222a0ca77679bb1b1fd9c1511 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 18 Sep 2017 22:20:35 +1200 Subject: [PATCH 122/138] Improved mixer code for crash flip mode. --- src/main/fc/fc_msp_box.c | 2 +- src/main/flight/mixer.c | 35 ++++++++++++++++++----------------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index ee87442e73..c35ad51ae6 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -76,7 +76,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXCAMERA1, "CAMERA CONTROL 1", 32}, { BOXCAMERA2, "CAMERA CONTROL 2", 33}, { BOXCAMERA3, "CAMERA CONTROL 3", 34 }, - { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH (DSHOT ONLY)", 35 }, + { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH", 35 }, { BOXPREARM, "PREARM", 36 }, }; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8577b08a6b..b6edc62c69 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -605,28 +605,29 @@ void calculateThrottleAndCurrentMotorEndpoints(void) throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); } +#define CRASH_FLIP_DEADBAND 20 + static void applyFlipOverAfterCrashModeToMotors(void) { float motorMix[MAX_SUPPORTED_MOTORS]; - for (int i = 0; i < motorCount; i++) { - if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { - motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * (-1); - } else { - motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * (-1); + if (ARMING_FLAG(ARMED)) { + for (int i = 0; i < motorCount; i++) { + if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { + motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1; + } else { + motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1; + } + + // Apply the mix to motor endpoints + float motorOutput = motorOutputMin + motorOutputRange * motorMix[i]; + //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered + motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND; + + motor[i] = motorOutput; } - } - // Apply the mix to motor endpoints - for (uint32_t i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i]); - //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered - motorOutput = (motorOutput < motorOutputMin + 20 ) ? disarmMotorOutput : motorOutput; - - motor[i] = motorOutput; - } - - // Disarmed mode - if (!ARMING_FLAG(ARMED)) { + } else { + // Disarmed mode for (int i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } From 268318eec6a018d21ab779019e327a3baa825d8a Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 23 Sep 2017 01:21:00 +0900 Subject: [PATCH 123/138] Touching up Disable debug variable assignment --- src/main/drivers/max7456.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 3ac56db359..2e43433404 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -455,10 +455,11 @@ void max7456Init(const vcdProfile_t *pVcdProfile) break; } - // XXX Disable for production +#ifdef DEBUG_MAX7456_SPI_CLOCK debug[0] = systemConfig()->cpu_overclock; debug[1] = max7456DeviceType; debug[2] = max7456SpiClock; +#endif #endif spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock); @@ -547,8 +548,6 @@ bool max7456DmaInProgress(void) #endif } -#include "build/debug.h" - void max7456DrawScreen(void) { uint8_t stallCheck; From 3f3b7fcf73b64f515fce186a76f5045f9f049f74 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 22 Sep 2017 19:10:26 -0700 Subject: [PATCH 124/138] EXTI2_TS_IRQHandler is actually only on F3, F4 startup files point to EXTI2_IRQHandler for pins on line 2 --- src/main/drivers/exti.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index f76f25d991..37de3d5780 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -207,9 +207,9 @@ void EXTI_IRQHandler(void) _EXTI_IRQ_HANDLER(EXTI0_IRQHandler); _EXTI_IRQ_HANDLER(EXTI1_IRQHandler); -#if defined(STM32F1) || defined(STM32F7) +#if defined(STM32F1) || defined(STM32F4) || defined(STM32F7) _EXTI_IRQ_HANDLER(EXTI2_IRQHandler); -#elif defined(STM32F3) || defined(STM32F4) +#elif defined(STM32F3) _EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler); #else # warning "Unknown CPU" From 53f5e87c7a15f6be305fc5c65be03407b3ee6fd3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 24 Sep 2017 08:20:33 +0100 Subject: [PATCH 125/138] Add void to function declarations/definitions where required --- src/main/blackbox/blackbox_io.c | 10 +++--- src/main/blackbox/blackbox_io.h | 4 +-- src/main/cms/cms_menu_blackbox.c | 2 +- src/main/cms/cms_menu_vtx_tramp.c | 4 +-- src/main/config/feature.c | 4 +-- src/main/config/parameter_group.c | 2 +- src/main/config/parameter_group.h | 2 +- src/main/drivers/camera_control.c | 6 ++-- src/main/drivers/camera_control.h | 2 +- src/main/drivers/cc2500.c | 2 +- src/main/drivers/cc2500.h | 2 +- src/main/drivers/compass/compass_ak8963.c | 2 +- src/main/drivers/compass/compass_ak8975.c | 2 +- src/main/drivers/sdcard.c | 4 +-- src/main/drivers/sdcard.h | 10 +++--- src/main/drivers/serial_usb_vcp.c | 3 +- src/main/fc/cli.c | 2 +- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_core.h | 2 +- src/main/flight/failsafe.h | 2 +- src/main/io/asyncfatfs/asyncfatfs.c | 32 +++++++++---------- src/main/io/asyncfatfs/asyncfatfs.h | 18 +++++------ src/main/io/dashboard.c | 6 ++-- src/main/io/flashfs.c | 31 +++++++++--------- src/main/io/flashfs.h | 24 +++++++------- src/main/io/ledstrip.c | 4 +-- src/main/io/osd.c | 2 +- src/main/io/rcsplit.c | 2 +- src/main/io/vtx_rtc6705.h | 2 +- src/main/io/vtx_smartaudio.c | 8 ++--- src/main/io/vtx_smartaudio.h | 2 +- src/main/io/vtx_tramp.c | 4 +-- src/main/io/vtx_tramp.h | 4 +-- src/main/msp/msp_serial.c | 2 +- src/main/rx/frsky_d.c | 8 ++--- src/main/rx/frsky_d.h | 2 +- src/main/rx/jetiexbus.c | 4 +-- src/main/sensors/gyroanalyse.c | 4 +-- src/main/sensors/gyroanalyse.h | 2 +- .../target/COLIBRI_RACE/bus_bst_stm32f30x.c | 8 ++--- src/main/target/SITL/target.h | 10 +++--- src/main/telemetry/hott.c | 2 +- src/main/telemetry/ltm.c | 4 +-- src/main/vcp/usb_pwr.c | 2 +- 45 files changed, 130 insertions(+), 128 deletions(-) diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 7741f5be0d..ba078afe63 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -55,7 +55,7 @@ static struct { #endif // USE_SDCARD -void blackboxOpen() +void blackboxOpen(void) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { @@ -351,7 +351,7 @@ static void blackboxLogFileCreated(afatfsFilePtr_t file) } } -static void blackboxCreateLogFile() +static void blackboxCreateLogFile(void) { uint32_t remainder = blackboxSDCard.largestLogFileNumber + 1; @@ -372,7 +372,7 @@ static void blackboxCreateLogFile() * * Keep calling until the function returns true (open is complete). */ -static bool blackboxSDCardBeginLog() +static bool blackboxSDCardBeginLog(void) { fatDirectoryEntry_t *directoryEntry; @@ -511,7 +511,7 @@ bool isBlackboxDeviceFull(void) } } -unsigned int blackboxGetLogNumber() +unsigned int blackboxGetLogNumber(void) { #ifdef USE_SDCARD return blackboxSDCard.largestLogFileNumber; @@ -523,7 +523,7 @@ unsigned int blackboxGetLogNumber() * Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can * transmit this iteration. */ -void blackboxReplenishHeaderBudget() +void blackboxReplenishHeaderBudget(void) { int32_t freeSpace; diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 06d5ef0e0c..4cfc0ca810 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -53,7 +53,7 @@ bool blackboxDeviceBeginLog(void); bool blackboxDeviceEndLog(bool retainLog); bool isBlackboxDeviceFull(void); -unsigned int blackboxGetLogNumber(); +unsigned int blackboxGetLogNumber(void); -void blackboxReplenishHeaderBudget(); +void blackboxReplenishHeaderBudget(void); blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes); diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 36f3dbdd23..1bcf7eb88d 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -69,7 +69,7 @@ static char cmsx_BlackboxStatus[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxDeviceStorageUsed[CMS_BLACKBOX_STRING_LENGTH]; static char cmsx_BlackboxDeviceStorageFree[CMS_BLACKBOX_STRING_LENGTH]; -static void cmsx_Blackbox_GetDeviceStatus() +static void cmsx_Blackbox_GetDeviceStatus(void) { char * unit = "B"; #if defined(USE_SDCARD) || defined(USE_FLASHFS) diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index b177274449..ff0f9afaf2 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -156,7 +156,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) return MENU_CHAIN_BACK; } -static void trampCmsInitSettings() +static void trampCmsInitSettings(void) { if (trampBand > 0) trampCmsBand = trampBand; if (trampChannel > 0) trampCmsChan = trampChannel; @@ -174,7 +174,7 @@ static void trampCmsInitSettings() } } -static long trampCmsOnEnter() +static long trampCmsOnEnter(void) { trampCmsInitSettings(); return 0; diff --git a/src/main/config/feature.c b/src/main/config/feature.c index bb5e5b9a75..8ee9c4afef 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -43,7 +43,7 @@ void intFeatureClearAll(uint32_t *features) *features = 0; } -void latchActiveFeatures() +void latchActiveFeatures(void) { activeFeaturesLatch = featureConfig()->enabledFeatures; } @@ -68,7 +68,7 @@ void featureClear(uint32_t mask) intFeatureClear(mask, &featureConfigMutable()->enabledFeatures); } -void featureClearAll() +void featureClearAll(void) { intFeatureClearAll(&featureConfigMutable()->enabledFeatures); } diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index 30bf48430a..a64d0eb973 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -85,7 +85,7 @@ int pgStore(const pgRegistry_t* reg, void *to, int size) return take; } -void pgResetAll() +void pgResetAll(void) { PG_FOREACH(reg) { pgReset(reg); diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index f64493ee87..9bee7b925f 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -186,7 +186,7 @@ const pgRegistry_t* pgFind(pgn_t pgn); void pgLoad(const pgRegistry_t* reg, const void *from, int size, int version); int pgStore(const pgRegistry_t* reg, void *to, int size); -void pgResetAll(); +void pgResetAll(void); void pgResetInstance(const pgRegistry_t *reg, uint8_t *base); bool pgResetCopy(void *copy, pgn_t pgn); void pgReset(const pgRegistry_t* reg); diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c index 82ef9ef0f7..50d68e818a 100644 --- a/src/main/drivers/camera_control.c +++ b/src/main/drivers/camera_control.c @@ -77,14 +77,14 @@ static struct { static uint32_t endTimeMillis; #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE -void TIM6_DAC_IRQHandler() +void TIM6_DAC_IRQHandler(void) { IOHi(cameraControlRuntime.io); TIM6->SR = 0; } -void TIM7_IRQHandler() +void TIM7_IRQHandler(void) { IOLo(cameraControlRuntime.io); @@ -92,7 +92,7 @@ void TIM7_IRQHandler() } #endif -void cameraControlInit() +void cameraControlInit(void) { if (cameraControlConfig()->ioTag == IO_TAG_NONE) return; diff --git a/src/main/drivers/camera_control.h b/src/main/drivers/camera_control.h index cafc58b955..031fb27aeb 100644 --- a/src/main/drivers/camera_control.h +++ b/src/main/drivers/camera_control.h @@ -49,7 +49,7 @@ typedef struct cameraControlConfig_s { PG_DECLARE(cameraControlConfig_t, cameraControlConfig); -void cameraControlInit(); +void cameraControlInit(void); void cameraControlProcess(uint32_t currentTimeUs); void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs); diff --git a/src/main/drivers/cc2500.c b/src/main/drivers/cc2500.c index 07aecd425d..b704244846 100644 --- a/src/main/drivers/cc2500.c +++ b/src/main/drivers/cc2500.c @@ -73,7 +73,7 @@ void cc2500SetPower(uint8_t power) cc2500WriteReg(CC2500_3E_PATABLE, patable[power]); } -uint8_t cc2500Reset() +uint8_t cc2500Reset(void) { cc2500Strobe(CC2500_SRES); delayMicroseconds(1000); // 1000us diff --git a/src/main/drivers/cc2500.h b/src/main/drivers/cc2500.h index 899ddb7247..a6cc96e1dc 100644 --- a/src/main/drivers/cc2500.h +++ b/src/main/drivers/cc2500.h @@ -150,4 +150,4 @@ uint8_t cc2500ReadReg(uint8_t reg); void cc2500Strobe(uint8_t address); uint8_t cc2500WriteReg(uint8_t address, uint8_t data); void cc2500SetPower(uint8_t power); -uint8_t cc2500Reset(); +uint8_t cc2500Reset(void); diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 99cedf6a16..40ca786940 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -152,7 +152,7 @@ static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) } #endif -static bool ak8963Init() +static bool ak8963Init(void) { uint8_t calibration[3]; uint8_t status; diff --git a/src/main/drivers/compass/compass_ak8975.c b/src/main/drivers/compass/compass_ak8975.c index f36596580e..52528f54e0 100644 --- a/src/main/drivers/compass/compass_ak8975.c +++ b/src/main/drivers/compass/compass_ak8975.c @@ -59,7 +59,7 @@ #define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value #define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value -static bool ak8975Init() +static bool ak8975Init(void) { uint8_t buffer[3]; uint8_t status; diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 3af9ee13a4..e2e8dd5f1c 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -632,7 +632,7 @@ static bool sdcard_setBlockLength(uint32_t blockLen) /* * Returns true if the card is ready to accept read/write commands. */ -static bool sdcard_isReady() +static bool sdcard_isReady(void) { return sdcard.state == SDCARD_STATE_READY || sdcard.state == SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; } @@ -647,7 +647,7 @@ static bool sdcard_isReady() * the SDCARD_READY state. * */ -static sdcardOperationStatus_e sdcard_endWriteBlocks() +static sdcardOperationStatus_e sdcard_endWriteBlocks(void) { sdcard.multiWriteBlocksRemain = 0; diff --git a/src/main/drivers/sdcard.h b/src/main/drivers/sdcard.h index cf101dd156..1dca004440 100644 --- a/src/main/drivers/sdcard.h +++ b/src/main/drivers/sdcard.h @@ -66,11 +66,11 @@ sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, void sdcardInsertionDetectDeinit(void); void sdcardInsertionDetectInit(void); -bool sdcard_isInserted(); -bool sdcard_isInitialized(); -bool sdcard_isFunctional(); +bool sdcard_isInserted(void); +bool sdcard_isInitialized(void); +bool sdcard_isFunctional(void); -bool sdcard_poll(); -const sdcardMetadata_t* sdcard_getMetadata(); +bool sdcard_poll(void); +const sdcardMetadata_t* sdcard_getMetadata(void); void sdcard_setProfilerCallback(sdcard_profilerCallback_c callback); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index dd70c8d028..fe9445dbdf 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -154,8 +154,9 @@ static void usbVcpBeginWrite(serialPort_t *instance) port->buffering = true; } -uint32_t usbTxBytesFree() +static uint32_t usbTxBytesFree(const serialPort_t *instance) { + UNUSED(instance); return CDC_Send_FreeBytes(); } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ccb95f36a1..853f6abcf1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -207,7 +207,7 @@ static void cliPrint(const char *str) bufWriterFlush(cliWriter); } -static void cliPrintLinefeed() +static void cliPrintLinefeed(void) { cliPrint("\r\n"); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 40d7fe1a13..d4460900fb 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -110,7 +110,7 @@ void setPreferredBeeperOffMask(uint32_t mask); void initEEPROM(void); void resetEEPROM(void); void readEEPROM(void); -void writeEEPROM(); +void writeEEPROM(void); void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index eb179c9893..8326a434d8 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -131,7 +131,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD saveConfigAndNotify(); } -static bool isCalibrating() +static bool isCalibrating(void) { #ifdef BARO if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index d9ac269c69..e42957633c 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -36,7 +36,7 @@ PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig); union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); -void handleInflightCalibrationStickPosition(); +void handleInflightCalibrationStickPosition(void); void resetArmingDisabled(void); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 581fe287ac..0d179fbcba 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -80,7 +80,7 @@ void failsafeReset(void); void failsafeStartMonitoring(void); void failsafeUpdateState(void); -failsafePhase_e failsafePhase(); +failsafePhase_e failsafePhase(void); bool failsafeIsMonitoring(void); bool failsafeIsActive(void); bool failsafeIsReceivingRxData(void); diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index d8d5c3cf2c..4400f4dc48 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -539,7 +539,7 @@ static bool afatfs_fileIsBusy(afatfsFilePtr_t file) * * Note that this is the same as the number of clusters in an AFATFS supercluster. */ -static uint32_t afatfs_fatEntriesPerSector() +static uint32_t afatfs_fatEntriesPerSector(void) { return afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT32 ? AFATFS_FAT32_FAT_ENTRIES_PER_SECTOR : AFATFS_FAT16_FAT_ENTRIES_PER_SECTOR; } @@ -548,7 +548,7 @@ static uint32_t afatfs_fatEntriesPerSector() * Size of a FAT cluster in bytes */ ONLY_EXPOSE_FOR_TESTING -uint32_t afatfs_clusterSize() +uint32_t afatfs_clusterSize(void) { return afatfs.sectorsPerCluster * AFATFS_SECTOR_SIZE; } @@ -806,7 +806,7 @@ static int afatfs_allocateCacheSector(uint32_t sectorIndex) /** * Attempt to flush dirty cache pages out to the sdcard, returning true if all flushable data has been flushed. */ -bool afatfs_flush() +bool afatfs_flush(void) { if (afatfs.cacheDirtyEntries > 0) { // Flush the oldest flushable sector @@ -836,7 +836,7 @@ bool afatfs_flush() /** * Returns true if either the freefile or the regular cluster pool has been exhausted during a previous write operation. */ -bool afatfs_isFull() +bool afatfs_isFull(void) { return afatfs.filesystemFull; } @@ -1618,7 +1618,7 @@ static afatfsOperationStatus_e afatfs_appendRegularFreeCluster(afatfsFilePtr_t f * Size of a AFATFS supercluster in bytes */ ONLY_EXPOSE_FOR_TESTING -uint32_t afatfs_superClusterSize() +uint32_t afatfs_superClusterSize(void) { return afatfs_fatEntriesPerSector() * afatfs_clusterSize(); } @@ -2364,7 +2364,7 @@ static afatfsOperationStatus_e afatfs_allocateDirectoryEntry(afatfsFilePtr_t dir * Return a pointer to a free entry in the open files table (a file whose type is "NONE"). You should initialize * the file afterwards with afatfs_initFileHandle(). */ -static afatfsFilePtr_t afatfs_allocateFileHandle() +static afatfsFilePtr_t afatfs_allocateFileHandle(void) { for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { if (afatfs.openFiles[i].type == AFATFS_FILE_TYPE_NONE) { @@ -3211,7 +3211,7 @@ static void afatfs_fileOperationContinue(afatfsFile_t *file) /** * Check files for pending operations and execute them. */ -static void afatfs_fileOperationsPoll() +static void afatfs_fileOperationsPoll(void) { afatfs_fileOperationContinue(&afatfs.currentDirectory); @@ -3229,7 +3229,7 @@ static void afatfs_fileOperationsPoll() /** * Return the available size of the freefile (used for files in contiguous append mode) */ -uint32_t afatfs_getContiguousFreeSpace() +uint32_t afatfs_getContiguousFreeSpace(void) { return afatfs.freeFile.logicalSize; } @@ -3238,7 +3238,7 @@ uint32_t afatfs_getContiguousFreeSpace() * Call to set up the initial state for finding the largest block of free space on the device whose corresponding FAT * sectors are themselves entirely free space (so the free space has dedicated FAT sectors of its own). */ -static void afatfs_findLargestContiguousFreeBlockBegin() +static void afatfs_findLargestContiguousFreeBlockBegin(void) { // The first FAT sector has two reserved entries, so it isn't eligible for this search. Start at the next FAT sector. afatfs.initState.freeSpaceSearch.candidateStart = afatfs_fatEntriesPerSector(); @@ -3256,7 +3256,7 @@ static void afatfs_findLargestContiguousFreeBlockBegin() * AFATFS_OPERATION_SUCCESS - When the search has finished and afatfs.initState.freeSpaceSearch has been updated with the details of the best gap. * AFATFS_OPERATION_FAILURE - When a read error occured */ -static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue() +static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue(void) { afatfsFreeSpaceSearch_t *opState = &afatfs.initState.freeSpaceSearch; uint32_t fatEntriesPerSector = afatfs_fatEntriesPerSector(); @@ -3361,7 +3361,7 @@ static void afatfs_introspecLogCreated(afatfsFile_t *file) #endif -static void afatfs_initContinue() +static void afatfs_initContinue(void) { #ifdef AFATFS_USE_FREEFILE afatfsOperationStatus_e status; @@ -3491,7 +3491,7 @@ static void afatfs_initContinue() * Check to see if there are any pending operations on the filesystem and perform a little work (without waiting on the * sdcard). You must call this periodically. */ -void afatfs_poll() +void afatfs_poll(void) { // Only attempt to continue FS operations if the card is present & ready, otherwise we would just be wasting time if (sdcard_poll()) { @@ -3554,17 +3554,17 @@ void afatfs_sdcardProfilerCallback(sdcardBlockOperation_e operation, uint32_t bl #endif -afatfsFilesystemState_e afatfs_getFilesystemState() +afatfsFilesystemState_e afatfs_getFilesystemState(void) { return afatfs.filesystemState; } -afatfsError_e afatfs_getLastError() +afatfsError_e afatfs_getLastError(void) { return afatfs.lastError; } -void afatfs_init() +void afatfs_init(void) { afatfs.filesystemState = AFATFS_FILESYSTEM_STATE_INITIALIZATION; afatfs.initPhase = AFATFS_INITIALIZATION_READ_MBR; @@ -3646,7 +3646,7 @@ bool afatfs_destroy(bool dirty) /** * Get a pessimistic estimate of the amount of buffer space that we have available to write to immediately. */ -uint32_t afatfs_getFreeBufferSpace() +uint32_t afatfs_getFreeBufferSpace(void) { uint32_t result = 0; for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { diff --git a/src/main/io/asyncfatfs/asyncfatfs.h b/src/main/io/asyncfatfs/asyncfatfs.h index faeb7ea510..40cd7f27d7 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.h +++ b/src/main/io/asyncfatfs/asyncfatfs.h @@ -58,7 +58,7 @@ typedef enum { } afatfsSeek_e; typedef void (*afatfsFileCallback_t)(afatfsFilePtr_t file); -typedef void (*afatfsCallback_t)(); +typedef void (*afatfsCallback_t)(void); bool afatfs_fopen(const char *filename, const char *mode, afatfsFileCallback_t complete); bool afatfs_ftruncate(afatfsFilePtr_t file, afatfsFileCallback_t callback); @@ -79,14 +79,14 @@ void afatfs_findFirst(afatfsFilePtr_t directory, afatfsFinder_t *finder); afatfsOperationStatus_e afatfs_findNext(afatfsFilePtr_t directory, afatfsFinder_t *finder, fatDirectoryEntry_t **dirEntry); void afatfs_findLast(afatfsFilePtr_t directory); -bool afatfs_flush(); -void afatfs_init(); +bool afatfs_flush(void); +void afatfs_init(void); bool afatfs_destroy(bool dirty); -void afatfs_poll(); +void afatfs_poll(void); -uint32_t afatfs_getFreeBufferSpace(); -uint32_t afatfs_getContiguousFreeSpace(); -bool afatfs_isFull(); +uint32_t afatfs_getFreeBufferSpace(void); +uint32_t afatfs_getContiguousFreeSpace(void); +bool afatfs_isFull(void); -afatfsFilesystemState_e afatfs_getFilesystemState(); -afatfsError_e afatfs_getLastError(); +afatfsFilesystemState_e afatfs_getFilesystemState(void); +afatfsError_e afatfs_getLastError(void); diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index c12b240eca..4d34a025a2 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -181,7 +181,7 @@ static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) } #if 0 -static void fillScreenWithCharacters() +static void fillScreenWithCharacters(void) { for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) { for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) { @@ -241,7 +241,7 @@ static void updateFailsafeStatus(void) i2c_OLED_send_char(bus, failsafeIndicator); } -static void showTitle() +static void showTitle(void) { i2c_OLED_set_line(bus, 0); i2c_OLED_send_string(bus, pageState.page->title); @@ -346,7 +346,7 @@ static void showProfilePage(void) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) #ifdef GPS -static void showGpsPage() +static void showGpsPage(void) { if (!feature(FEATURE_GPS)) { pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c index 5e346d1b82..3d0b882bef 100644 --- a/src/main/io/flashfs.c +++ b/src/main/io/flashfs.c @@ -52,12 +52,12 @@ static uint8_t bufferHead = 0, bufferTail = 0; // The position of the buffer's tail in the overall flash address space: static uint32_t tailAddress = 0; -static void flashfsClearBuffer() +static void flashfsClearBuffer(void) { bufferTail = bufferHead = 0; } -static bool flashfsBufferIsEmpty() +static bool flashfsBufferIsEmpty(void) { return bufferTail == bufferHead; } @@ -67,7 +67,7 @@ static void flashfsSetTailAddress(uint32_t address) tailAddress = address; } -void flashfsEraseCompletely() +void flashfsEraseCompletely(void) { m25p16_eraseCompletely(); @@ -106,17 +106,17 @@ void flashfsEraseRange(uint32_t start, uint32_t end) /** * Return true if the flash is not currently occupied with an operation. */ -bool flashfsIsReady() +bool flashfsIsReady(void) { return m25p16_isReady(); } -uint32_t flashfsGetSize() +uint32_t flashfsGetSize(void) { return m25p16_getGeometry()->totalSize; } -static uint32_t flashfsTransmitBufferUsed() +static uint32_t flashfsTransmitBufferUsed(void) { if (bufferHead >= bufferTail) return bufferHead - bufferTail; @@ -127,7 +127,7 @@ static uint32_t flashfsTransmitBufferUsed() /** * Get the size of the largest single write that flashfs could ever accept without blocking or data loss. */ -uint32_t flashfsGetWriteBufferSize() +uint32_t flashfsGetWriteBufferSize(void) { return FLASHFS_WRITE_BUFFER_USABLE; } @@ -135,12 +135,12 @@ uint32_t flashfsGetWriteBufferSize() /** * Get the number of bytes that can currently be written to flashfs without any blocking or data loss. */ -uint32_t flashfsGetWriteBufferFreeSpace() +uint32_t flashfsGetWriteBufferFreeSpace(void) { return flashfsGetWriteBufferSize() - flashfsTransmitBufferUsed(); } -const flashGeometry_t* flashfsGetGeometry() +const flashGeometry_t* flashfsGetGeometry(void) { return m25p16_getGeometry(); } @@ -270,7 +270,7 @@ static void flashfsGetDirtyDataBuffers(uint8_t const *buffers[], uint32_t buffer /** * Get the current offset of the file pointer within the volume. */ -uint32_t flashfsGetOffset() +uint32_t flashfsGetOffset(void) { uint8_t const * buffers[2]; uint32_t bufferSizes[2]; @@ -305,7 +305,7 @@ static void flashfsAdvanceTailInBuffer(uint32_t delta) * Returns true if all data in the buffer has been flushed to the device, or false if * there is still data to be written (call flush again later). */ -bool flashfsFlushAsync() +bool flashfsFlushAsync(void) { if (flashfsBufferIsEmpty()) { return true; // Nothing to flush @@ -328,7 +328,7 @@ bool flashfsFlushAsync() * The flash will still be busy some time after this sync completes, but space will * be freed up to accept more writes in the write buffer. */ -void flashfsFlushSync() +void flashfsFlushSync(void) { if (flashfsBufferIsEmpty()) { return; // Nothing to flush @@ -484,7 +484,7 @@ int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len) /** * Find the offset of the start of the free space on the device (or the size of the device if it is full). */ -int flashfsIdentifyStartOfFreeSpace() +int flashfsIdentifyStartOfFreeSpace(void) { /* Find the start of the free space on the device by examining the beginning of blocks with a binary search, * looking for ones that appear to be erased. We can achieve this with good accuracy because an erased block @@ -553,14 +553,15 @@ int flashfsIdentifyStartOfFreeSpace() /** * Returns true if the file pointer is at the end of the device. */ -bool flashfsIsEOF() { +bool flashfsIsEOF(void) +{ return tailAddress >= flashfsGetSize(); } /** * Call after initializing the flash chip in order to set up the filesystem. */ -void flashfsInit() +void flashfsInit(void) { // If we have a flash chip present at all if (flashfsGetSize() > 0) { diff --git a/src/main/io/flashfs.h b/src/main/io/flashfs.h index 90f1ca9e57..216c8c22e0 100644 --- a/src/main/io/flashfs.h +++ b/src/main/io/flashfs.h @@ -23,16 +23,16 @@ // Automatically trigger a flush when this much data is in the buffer #define FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN 64 -void flashfsEraseCompletely(); +void flashfsEraseCompletely(void); void flashfsEraseRange(uint32_t start, uint32_t end); -uint32_t flashfsGetSize(); -uint32_t flashfsGetOffset(); -uint32_t flashfsGetWriteBufferFreeSpace(); -uint32_t flashfsGetWriteBufferSize(); -int flashfsIdentifyStartOfFreeSpace(); +uint32_t flashfsGetSize(void); +uint32_t flashfsGetOffset(void); +uint32_t flashfsGetWriteBufferFreeSpace(void); +uint32_t flashfsGetWriteBufferSize(void); +int flashfsIdentifyStartOfFreeSpace(void); struct flashGeometry_s; -const struct flashGeometry_s* flashfsGetGeometry(); +const struct flashGeometry_s* flashfsGetGeometry(void); void flashfsSeekAbs(uint32_t offset); void flashfsSeekRel(int32_t offset); @@ -42,10 +42,10 @@ void flashfsWrite(const uint8_t *data, unsigned int len, bool sync); int flashfsReadAbs(uint32_t offset, uint8_t *data, unsigned int len); -bool flashfsFlushAsync(); -void flashfsFlushSync(); +bool flashfsFlushAsync(void); +void flashfsFlushSync(void); -void flashfsInit(); +void flashfsInit(void); -bool flashfsIsReady(); -bool flashfsIsEOF(); +bool flashfsIsReady(void); +bool flashfsIsEOF(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 2f6437ef28..607cd5dd9f 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -441,7 +441,7 @@ static const struct { {0, LED_MODE_ORIENTATION}, }; -static void applyLedFixedLayers() +static void applyLedFixedLayers(void) { for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[ledIndex]; @@ -1146,7 +1146,7 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) return true; } -void ledStripInit() +void ledStripInit(void) { colors = ledStripConfigMutable()->colors; modeColors = ledStripConfig()->modeColors; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f8478aa5c8..8be67ee857 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -163,7 +163,7 @@ static escSensorData_t *escData; /** * Gets the correct altitude symbol for the current unit system */ -static char osdGetMetersToSelectedUnitSymbol() +static char osdGetMetersToSelectedUnitSymbol(void) { switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: diff --git a/src/main/io/rcsplit.c b/src/main/io/rcsplit.c index d567b2afd6..aada0181ab 100644 --- a/src/main/io/rcsplit.c +++ b/src/main/io/rcsplit.c @@ -83,7 +83,7 @@ static void sendCtrlCommand(rcsplit_ctrl_argument_e argument) serialWriteBuf(rcSplitSerialPort, uart_buffer, 5); } -static void rcSplitProcessMode() +static void rcSplitProcessMode(void) { // if the device not ready, do not handle any mode change event if (RCSPLIT_STATE_IS_READY != cameraState) diff --git a/src/main/io/vtx_rtc6705.h b/src/main/io/vtx_rtc6705.h index 9a48a8aa03..1e304f682b 100644 --- a/src/main/io/vtx_rtc6705.h +++ b/src/main/io/vtx_rtc6705.h @@ -41,4 +41,4 @@ PG_DECLARE(vtxRTC6705Config_t, vtxRTC6705Config); extern const char * const rtc6705PowerNames[RTC6705_POWER_COUNT]; void vtxRTC6705Configure(void); -bool vtxRTC6705Init(); +bool vtxRTC6705Init(void); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 1fadb13685..d71e783da9 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -491,7 +491,7 @@ static uint8_t sa_qhead = 0; static uint8_t sa_qtail = 0; #ifdef DPRINTF_SMARTAUDIO -static int saQueueLength() +static int saQueueLength(void) { if (sa_qhead >= sa_qtail) { return sa_qhead - sa_qtail; @@ -501,12 +501,12 @@ static int saQueueLength() } #endif -static bool saQueueEmpty() +static bool saQueueEmpty(void) { return sa_qhead == sa_qtail; } -static bool saQueueFull() +static bool saQueueFull(void) { return ((sa_qhead + 1) % SA_QSIZE) == sa_qtail; } @@ -609,7 +609,7 @@ void saSetPowerByIndex(uint8_t index) saQueueCmd(buf, 6); } -bool vtxSmartAudioInit() +bool vtxSmartAudioInit(void) { #ifdef SMARTAUDIO_DPRINTF // Setup debugSerialPort diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 0fd31dad80..8cbd0fc9b7 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -60,7 +60,7 @@ void saSetBandAndChannel(uint8_t band, uint8_t channel); void saSetMode(int mode); void saSetPowerByIndex(uint8_t index); void saSetFreq(uint16_t freq); -bool vtxSmartAudioInit(); +bool vtxSmartAudioInit(void); #ifdef SMARTAUDIO_DPRINTF #ifdef OMNIBUSF4 diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 73eaef16db..53dabdf7bd 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -157,7 +157,7 @@ void trampSendRFPower(uint16_t level) } // return false if error -bool trampCommitChanges() +bool trampCommitChanges(void) { if (trampStatus != TRAMP_STATUS_ONLINE) return false; @@ -233,7 +233,7 @@ typedef enum { static trampReceiveState_e trampReceiveState = S_WAIT_LEN; static int trampReceivePos = 0; -static void trampResetReceiver() +static void trampResetReceiver(void) { trampReceiveState = S_WAIT_LEN; trampReceivePos = 0; diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 094f2b0d75..3e388e1b05 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -12,8 +12,8 @@ extern uint32_t trampCurFreq; extern uint16_t trampConfiguredPower; // Configured transmitting power extern int16_t trampTemperature; -bool vtxTrampInit(); -bool trampCommitChanges(); +bool vtxTrampInit(void); +bool trampCommitChanges(void); void trampSetPitMode(uint8_t onoff); void trampSetBandAndChannel(uint8_t band, uint8_t channel); void trampSetRFPower(uint16_t level); diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 7e05410cfc..9500296b40 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -301,7 +301,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direct } -uint32_t mspSerialTxBytesFree() +uint32_t mspSerialTxBytesFree(void) { uint32_t ret = UINT32_MAX; diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c index f93c33a503..4308440d61 100644 --- a/src/main/rx/frsky_d.c +++ b/src/main/rx/frsky_d.c @@ -193,25 +193,25 @@ static void telemetry_build_frame(uint8_t *packet) #endif #if defined(USE_FRSKY_RX_PA_LNA) -static void RX_enable() +static void RX_enable(void) { IOLo(txEnPin); IOHi(rxEnPin); } -static void TX_enable() +static void TX_enable(void) { IOLo(rxEnPin); IOHi(txEnPin); } #endif -void frSkyDBind() +void frSkyDBind(void) { bindRequested = true; } -static void initialize() +static void initialize(void) { cc2500Reset(); cc2500WriteReg(CC2500_02_IOCFG0, 0x01); diff --git a/src/main/rx/frsky_d.h b/src/main/rx/frsky_d.h index d7c661b130..c565f8943f 100644 --- a/src/main/rx/frsky_d.h +++ b/src/main/rx/frsky_d.h @@ -36,4 +36,4 @@ struct rxRuntimeConfig_s; void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload); rx_spi_received_e frSkyDDataReceived(uint8_t *payload); -void frSkyDBind(); +void frSkyDBind(void); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 4f13e61fbe..f83070792e 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -281,7 +281,7 @@ void jetiExBusDecodeChannelFrame(uint8_t *exBusFrame) } -void jetiExBusFrameReset() +void jetiExBusFrameReset(void) { jetiExBusFramePosition = 0; jetiExBusFrameLength = EXBUS_MAX_CHANNEL_FRAME_SIZE; @@ -376,7 +376,7 @@ static void jetiExBusDataReceive(uint16_t c) // Check if it is time to read a frame from the data... -static uint8_t jetiExBusFrameStatus() +static uint8_t jetiExBusFrameStatus(void) { if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) return RX_FRAME_PENDING; diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index b680a131b3..dd741bc973 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -86,14 +86,14 @@ static biquadFilter_t fftFreqFilter[3]; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static float hanningWindow[FFT_WINDOW_SIZE]; -void initHanning() +void initHanning(void) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) { hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); } } -void initGyroData() +void initGyroData(void) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) { diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index ac4290738a..a4a64cd431 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -31,4 +31,4 @@ const gyroFftData_t *gyroFftData(int axis); struct gyroDev_s; void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn); void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); -bool isDynamicFilterActive(); +bool isDynamicFilterActive(void); diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 2b18e740cd..cf87516a87 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -71,7 +71,7 @@ uint8_t interruptCounter = 0; #define DELAY_SENDING_BYTE 40 void bstProcessInCommand(void); -void I2C_EV_IRQHandler() +void I2C_EV_IRQHandler(void) { if (I2C_GetITStatus(BSTx, I2C_IT_ADDR)) { CRC8 = 0; @@ -154,17 +154,17 @@ void I2C_EV_IRQHandler() } } -void I2C1_EV_IRQHandler() +void I2C1_EV_IRQHandler(void) { I2C_EV_IRQHandler(); } -void I2C2_EV_IRQHandler() +void I2C2_EV_IRQHandler(void) { I2C_EV_IRQHandler(); } -uint32_t bstTimeoutUserCallback() +uint32_t bstTimeoutUserCallback(void) { bstErrorCount++; diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 9bb9b4a327..e5ccbc57e0 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -250,11 +250,11 @@ void FLASH_Lock(void); FLASH_Status FLASH_ErasePage(uintptr_t Page_Address); FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data); -uint64_t nanos64_real(); -uint64_t micros64_real(); -uint64_t millis64_real(); +uint64_t nanos64_real(void); +uint64_t micros64_real(void); +uint64_t millis64_real(void); void delayMicroseconds_real(uint32_t us); -uint64_t micros64(); -uint64_t millis64(); +uint64_t micros64(void); +uint64_t millis64(void); int lockMainPID(void); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 27d84c223a..a956aebfcb 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -527,7 +527,7 @@ static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros) return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ; } -static inline bool shouldCheckForHoTTRequest() +static inline bool shouldCheckForHoTTRequest(void) { if (hottIsSending) { return false; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index c4cfed8188..4292d386f9 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -202,7 +202,7 @@ static void ltm_sframe(void) * Attitude A-frame - 10 Hz at > 2400 baud * PITCH ROLL HEADING */ -static void ltm_aframe() +static void ltm_aframe(void) { ltm_initialise_packet('A'); ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch)); @@ -216,7 +216,7 @@ static void ltm_aframe() * This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD * home pos, home alt, direction to home */ -static void ltm_oframe() +static void ltm_oframe(void) { ltm_initialise_packet('O'); #if defined(GPS) diff --git a/src/main/vcp/usb_pwr.c b/src/main/vcp/usb_pwr.c index 8cdd7eb3d1..e383ab2c81 100644 --- a/src/main/vcp/usb_pwr.c +++ b/src/main/vcp/usb_pwr.c @@ -88,7 +88,7 @@ RESULT PowerOn(void) * Output : None. * Return : USB_SUCCESS. *******************************************************************************/ -RESULT PowerOff() +RESULT PowerOff(void) { /* disable all interrupts and force USB reset */ _SetCNTR(CNTR_FRES); From 19230a436fa386a5b6da094fa99d41525566fffb Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 24 Sep 2017 08:04:31 +0100 Subject: [PATCH 126/138] Tidy of VTX code --- src/main/io/vtx_control.c | 34 +++++++++--------- src/main/io/vtx_control.h | 6 ++++ src/main/io/vtx_rtc6705.c | 4 +-- src/main/io/vtx_rtc6705.h | 2 ++ src/main/io/vtx_smartaudio.c | 68 +++++++++++++++++++++-------------- src/main/io/vtx_smartaudio.h | 5 +++ src/main/io/vtx_string.c | 10 ++---- src/main/io/vtx_string.h | 2 ++ src/main/io/vtx_tramp.c | 69 +++++++++++++++++++++--------------- src/main/io/vtx_tramp.h | 2 ++ 10 files changed, 122 insertions(+), 80 deletions(-) diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index eb78d18781..17de43bb16 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -16,9 +16,13 @@ */ -// Get target build configuration +#include +#include + #include "platform.h" +#if defined(VTX_CONTROL) && defined(VTX_COMMON) + #include "common/maths.h" #include "config/config_eeprom.h" @@ -38,7 +42,6 @@ #include "io/vtx_control.h" -#if defined(VTX_CONTROL) && defined(VTX_COMMON) PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); @@ -176,7 +179,6 @@ void vtxCyclePower(const uint8_t powerStep) void handleVTXControlButton(void) { #if defined(VTX_RTC6705) && defined(BUTTON_A_PIN) - bool buttonHeld; bool buttonWasPressed = false; uint32_t start = millis(); uint32_t ledToggleAt = start; @@ -184,6 +186,7 @@ void handleVTXControlButton(void) uint8_t flashesDone = 0; uint8_t actionCounter = 0; + bool buttonHeld; while ((buttonHeld = buttonAPressed())) { uint32_t end = millis(); @@ -232,21 +235,20 @@ void handleVTXControlButton(void) LED1_OFF; switch (actionCounter) { - case 4: - vtxCycleBandOrChannel(0, +1); - break; - case 3: - vtxCycleBandOrChannel(+1, 0); - break; - case 2: - vtxCyclePower(+1); - break; - case 1: - saveConfigAndNotify(); - break; + case 4: + vtxCycleBandOrChannel(0, +1); + break; + case 3: + vtxCycleBandOrChannel(+1, 0); + break; + case 2: + vtxCyclePower(+1); + break; + case 1: + saveConfigAndNotify(); + break; } #endif } #endif - diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 9c1e19eb8a..66c202d446 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -17,6 +17,12 @@ #pragma once +#include +#include + +#include "platform.h" + +#include "config/parameter_group.h" #include "fc/rc_modes.h" #define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10 diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index f79e164e13..1b387fff4f 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/src/main/io/vtx_rtc6705.c @@ -25,7 +25,9 @@ #include "platform.h" #if defined(VTX_RTC6705) && defined(VTX_CONTROL) + #include "build/build_config.h" +#include "build/debug.h" #include "cms/cms.h" #include "cms/cms_types.h" @@ -46,8 +48,6 @@ #include "io/vtx_rtc6705.h" #include "io/vtx_string.h" -#include "build/debug.h" - bool canUpdateVTX(void); PG_REGISTER_WITH_RESET_TEMPLATE(vtxRTC6705Config_t, vtxRTC6705Config, PG_VTX_RTC6705_CONFIG, 0); diff --git a/src/main/io/vtx_rtc6705.h b/src/main/io/vtx_rtc6705.h index 1e304f682b..002800fea8 100644 --- a/src/main/io/vtx_rtc6705.h +++ b/src/main/io/vtx_rtc6705.h @@ -20,6 +20,8 @@ #include #include +#include "platform.h" + #include "config/parameter_group.h" typedef struct vtxRTC6705Config_s { diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index d71e783da9..61b1fef5fa 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -25,10 +25,10 @@ #include "platform.h" #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) + #include "build/build_config.h" #include "build/debug.h" - #include "cms/cms.h" #include "cms/cms_types.h" #include "cms/cms_menu_vtx_smartaudio.h" @@ -194,8 +194,9 @@ static void saPrintSettings(void) int saDacToPowerIndex(int dac) { for (int idx = 3 ; idx >= 0 ; idx--) { - if (saPowerTable[idx].valueV1 <= dac) + if (saPowerTable[idx].valueV1 <= dac) { return idx; + } } return 0; } @@ -214,9 +215,10 @@ static int sa_baudstep = 50; static void saAutobaud(void) { - if (saStat.pktsent < 10) + if (saStat.pktsent < 10) { // Not enough samples collected return; + } #if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", @@ -277,8 +279,9 @@ static void saProcessResponse(uint8_t *buf, int len) switch (resp) { case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings - if (len < 7) + if (len < 7) { break; + } saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; saDevice.channel = buf[2]; @@ -301,10 +304,11 @@ static void saProcessResponse(uint8_t *buf, int len) break; case SA_CMD_SET_FREQ: // Set Frequency - if (len < 5) + if (len < 5) { break; + } - uint16_t freq = (buf[2] << 8)|buf[3]; + const uint16_t freq = (buf[2] << 8)|buf[3]; if (freq & SA_FREQ_GETPIT) { saDevice.orfreq = freq & SA_FREQ_MASK; @@ -328,8 +332,9 @@ static void saProcessResponse(uint8_t *buf, int len) } // Debug - if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) + if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) { saPrintSettings(); + } saDevicePrev = saDevice; #ifdef VTX_COMMON @@ -427,12 +432,11 @@ static void saReceiveFramer(uint8_t c) static void saSendFrame(uint8_t *buf, int len) { - int i; - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { serialWrite(smartAudioSerialPort, buf[i]); + } serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this @@ -467,10 +471,9 @@ static void saResendCmd(void) static void saSendCmd(uint8_t *buf, int len) { - int i; - - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { sa_osbuf[i] = buf[i]; + } sa_oslen = len; sa_outstanding = (buf[2] >> 1); @@ -513,8 +516,9 @@ static bool saQueueFull(void) static void saQueueCmd(uint8_t *buf, int len) { - if (saQueueFull()) + if (saQueueFull()) { return; + } sa_queue[sa_qhead].buf = buf; sa_queue[sa_qhead].len = len; @@ -523,8 +527,9 @@ static void saQueueCmd(uint8_t *buf, int len) static void saSendQueue(void) { - if (saQueueEmpty()) + if (saQueueEmpty()) { return; + } saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len); sa_qtail = (sa_qtail + 1) % SA_QSIZE; @@ -601,8 +606,9 @@ void saSetPowerByIndex(uint8_t index) return; } - if (index > 3) + if (index > 3) { return; + } buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; buf[5] = CRC8(buf, 5); @@ -646,8 +652,9 @@ void vtxSAProcess(uint32_t now) { static char initPhase = 0; - if (smartAudioSerialPort == NULL) + if (smartAudioSerialPort == NULL) { return; + } while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { uint8_t c = serialRead(smartAudioSerialPort); @@ -673,8 +680,7 @@ void vtxSAProcess(uint32_t now) break; } - if ((sa_outstanding != SA_CMD_NONE) - && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { + if ((sa_outstanding != SA_CMD_NONE) && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); // XXX Todo: Resend termination and possible offline transition @@ -694,8 +700,9 @@ void vtxSAProcess(uint32_t now) // Testing VTX_COMMON API { static uint32_t lastMonitorUs = 0; - if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) + if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) { return; + } static uint8_t monBand; static uint8_t monChan; @@ -725,8 +732,9 @@ bool vtxSAIsReady(void) void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) { - if (band && channel) + if (band && channel) { saSetBandAndChannel(band - 1, channel - 1); + } } void vtxSASetPowerByIndex(uint8_t index) @@ -741,8 +749,9 @@ void vtxSASetPowerByIndex(uint8_t index) void vtxSASetPitMode(uint8_t onoff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady() && (saDevice.version == 2))) { return; + } if (onoff) { // SmartAudio can not turn pit mode on by software. @@ -751,11 +760,13 @@ void vtxSASetPitMode(uint8_t onoff) uint8_t newmode = SA_MODE_CLR_PITMODE; - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { newmode |= SA_MODE_SET_IN_RANGE_PITMODE; + } - if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) { newmode |= SA_MODE_SET_OUT_RANGE_PITMODE; + } saSetMode(newmode); @@ -764,8 +775,9 @@ void vtxSASetPitMode(uint8_t onoff) bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) { - if (!vtxSAIsReady()) + if (!vtxSAIsReady()) { return false; + } *pBand = (saDevice.channel / 8) + 1; *pChannel = (saDevice.channel % 8) + 1; @@ -774,8 +786,9 @@ bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxSAGetPowerIndex(uint8_t *pIndex) { - if (!vtxSAIsReady()) + if (!vtxSAIsReady()) { return false; + } *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; return true; @@ -783,8 +796,9 @@ bool vtxSAGetPowerIndex(uint8_t *pIndex) bool vtxSAGetPitMode(uint8_t *pOnOff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady() && (saDevice.version == 2))) { return false; + } *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0; return true; diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 8cbd0fc9b7..a4a0cf820d 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -1,5 +1,10 @@ #pragma once +#include +#include + +#include "platform.h" + // opmode flags, GET side #define SA_MODE_GET_FREQ_BY_FREQ 1 #define SA_MODE_GET_PITMODE 2 diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index 71f0e4047a..c341a1699f 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -19,10 +19,9 @@ #include #include -#include -#include #include "platform.h" + #include "build/debug.h" #if defined(VTX_COMMON) @@ -53,13 +52,10 @@ const char * const vtx58ChannelNames[] = { bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) { - int8_t band; - uint8_t channel; - // Use reverse lookup order so that 5880Mhz // get Raceband 7 instead of Fatshark 8. - for (band = 4 ; band >= 0 ; band--) { - for (channel = 0 ; channel < 8 ; channel++) { + for (int band = 4 ; band >= 0 ; band--) { + for (int channel = 0 ; channel < 8 ; channel++) { if (vtx58frequencyTable[band][channel] == freq) { *pBand = band + 1; *pChannel = channel + 1; diff --git a/src/main/io/vtx_string.h b/src/main/io/vtx_string.h index a3074a8966..736ac8daa3 100644 --- a/src/main/io/vtx_string.h +++ b/src/main/io/vtx_string.h @@ -2,6 +2,8 @@ #include +#include "platform.h" + #if defined(VTX_COMMON) extern const uint16_t vtx58frequencyTable[5][8]; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 53dabdf7bd..09e9fb4e9e 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -107,16 +107,18 @@ static uint8_t trampChecksum(uint8_t *trampBuf) { uint8_t cksum = 0; - for (int i = 1 ; i < 14 ; i++) + for (int i = 1 ; i < 14 ; i++) { cksum += trampBuf[i]; + } return cksum; } void trampCmdU16(uint8_t cmd, uint16_t param) { - if (!trampSerialPort) + if (!trampSerialPort) { return; + } memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); trampReqBuffer[0] = 15; @@ -130,8 +132,9 @@ void trampCmdU16(uint8_t cmd, uint16_t param) void trampSetFreq(uint16_t freq) { trampConfFreq = freq; - if (trampConfFreq != trampCurFreq) + if (trampConfFreq != trampCurFreq) { trampFreqRetries = TRAMP_MAX_RETRIES; + } } void trampSendFreq(uint16_t freq) @@ -147,8 +150,9 @@ void trampSetBandAndChannel(uint8_t band, uint8_t channel) void trampSetRFPower(uint16_t level) { trampConfPower = level; - if (trampConfPower != trampPower) + if (trampConfPower != trampPower) { trampPowerRetries = TRAMP_MAX_RETRIES; + } } void trampSendRFPower(uint16_t level) @@ -159,8 +163,9 @@ void trampSendRFPower(uint16_t level) // return false if error bool trampCommitChanges(void) { - if (trampStatus != TRAMP_STATUS_ONLINE) + if (trampStatus != TRAMP_STATUS_ONLINE) { return false; + } trampStatus = TRAMP_STATUS_SET_FREQ_PW; return true; @@ -174,12 +179,12 @@ void trampSetPitMode(uint8_t onoff) // returns completed response code char trampHandleResponse(void) { - uint8_t respCode = trampRespBuffer[1]; + const uint8_t respCode = trampRespBuffer[1]; switch (respCode) { case 'r': { - uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); if (min_freq != 0) { trampRFFreqMin = min_freq; trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); @@ -193,7 +198,7 @@ char trampHandleResponse(void) case 'v': { - uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); if (freq != 0) { trampCurFreq = freq; trampConfiguredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); @@ -212,7 +217,7 @@ char trampHandleResponse(void) case 's': { - uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); + const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); if (temp != 0) { trampTemperature = temp; return 's'; @@ -241,10 +246,11 @@ static void trampResetReceiver(void) static bool trampIsValidResponseCode(uint8_t code) { - if (code == 'r' || code == 'v' || code == 's') + if (code == 'r' || code == 'v' || code == 's') { return true; - else + } else { return false; + } } // returns completed response code or 0 @@ -252,11 +258,12 @@ static char trampReceive(uint32_t currentTimeUs) { UNUSED(currentTimeUs); - if (!trampSerialPort) + if (!trampSerialPort) { return 0; + } while (serialRxBytesWaiting(trampSerialPort)) { - uint8_t c = serialRead(trampSerialPort); + const uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; switch (trampReceiveState) { @@ -290,6 +297,7 @@ static char trampReceive(uint32_t currentTimeUs) default: trampResetReceiver(); + break; } } @@ -326,10 +334,11 @@ void vtxTrampProcess(uint32_t currentTimeUs) static uint16_t debugPowReqCounter = 0; #endif - if (trampStatus == TRAMP_STATUS_BAD_DEVICE) + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) { return; + } - char replyCode = trampReceive(currentTimeUs); + const char replyCode = trampReceive(currentTimeUs); #ifdef TRAMP_DEBUG debug[0] = trampStatus; @@ -337,13 +346,15 @@ void vtxTrampProcess(uint32_t currentTimeUs) switch (replyCode) { case 'r': - if (trampStatus <= TRAMP_STATUS_OFFLINE) + if (trampStatus <= TRAMP_STATUS_OFFLINE) { trampStatus = TRAMP_STATUS_ONLINE; + } break; case 'v': - if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) { trampStatus = TRAMP_STATUS_SET_FREQ_PW; + } break; } @@ -353,14 +364,15 @@ void vtxTrampProcess(uint32_t currentTimeUs) case TRAMP_STATUS_ONLINE: if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s - if (trampStatus == TRAMP_STATUS_OFFLINE) + if (trampStatus == TRAMP_STATUS_OFFLINE) { trampQueryR(); - else { + } else { static unsigned int cnt = 0; - if (((cnt++) & 1) == 0) + if (((cnt++) & 1) == 0) { trampQueryV(); - else + } else { trampQueryS(); + } } lastQueryTimeUs = currentTimeUs; @@ -377,8 +389,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) debugFreqReqCounter++; #endif done = false; - } - else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) { + } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampConfiguredPower)) { trampSendRFPower(trampConfPower); trampPowerRetries--; #ifdef TRAMP_DEBUG @@ -392,8 +403,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) // delay next status query by 300ms lastQueryTimeUs = currentTimeUs + 300 * 1000; - } - else { + } else { // everything has been done, let's return to original state trampStatus = TRAMP_STATUS_ONLINE; // reset configuration value in case it failed (no more retries) @@ -464,8 +474,9 @@ void vtxTrampSetPitMode(uint8_t onoff) bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady()) { return false; + } *pBand = trampBand; *pChannel = trampChannel; @@ -474,8 +485,9 @@ bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxTrampGetPowerIndex(uint8_t *pIndex) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady()) { return false; + } if (trampConfiguredPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { @@ -491,8 +503,9 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) bool vtxTrampGetPitMode(uint8_t *pOnOff) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady()) { return false; + } *pOnOff = trampPitMode; return true; diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 3e388e1b05..edb420cd1b 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -1,5 +1,7 @@ #pragma once +#include + #define VTX_TRAMP_POWER_COUNT 5 extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT]; extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1]; From 68ceaa4558984d65d43bd6c3ada2833172d20039 Mon Sep 17 00:00:00 2001 From: Faduf Date: Sun, 24 Sep 2017 16:57:06 +0200 Subject: [PATCH 127/138] Add specific settings to YupiF4 for Version 3 --- src/main/target/YUPIF4/config.c | 13 ++++++++++--- src/main/target/YUPIF4/hardware_revision.c | 7 +++++-- src/main/target/YUPIF4/hardware_revision.h | 3 ++- src/main/target/YUPIF4/target.h | 2 ++ 4 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index 15d5ce0771..19d70e8a7a 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -22,26 +22,33 @@ #ifdef TARGET_CONFIG #include "blackbox/blackbox.h" - #include "fc/config.h" - #include "flight/pid.h" +#include "telemetry/telemetry.h" #include "hardware_revision.h" + // alternative defaults settings for YuPiF4 targets void targetConfiguration(void) { /* Changes depending on versions */ - if (hardwareRevision == YUPIF4_RACE2) { + if (hardwareRevision == YUPIF4_RACE3) { + beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + telemetryConfigMutable()->halfDuplex = false; + + } else if (hardwareRevision == YUPIF4_RACE2) { beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + } else if (hardwareRevision == YUPIF4_MINI) { beeperDevConfigMutable()->frequency = 0; blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE; adcConfigMutable()->current.enabled = 0; + } else if (hardwareRevision == YUPIF4_NAV) { beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + } else { adcConfigMutable()->current.enabled = 0; } diff --git a/src/main/target/YUPIF4/hardware_revision.c b/src/main/target/YUPIF4/hardware_revision.c index c140355207..5e21a8351d 100644 --- a/src/main/target/YUPIF4/hardware_revision.c +++ b/src/main/target/YUPIF4/hardware_revision.c @@ -52,13 +52,16 @@ void detectHardwareRevision(void) no Hardware pins tied to ground => Race V1 if Pin 1 is the only one tied to ground => Mini if Pin 2 is the only one tied to ground => Race V2 + if Pin 2 and Pin 1 are tied to ground => Race V3 if Pin 3 is the only one tied to ground => Navigation Other combinations available for potential evolutions */ - if (!IORead(pin1)) { + if (!IORead(pin1) && IORead(pin2)) { hardwareRevision = YUPIF4_MINI; - } else if (!IORead(pin2)) { + } else if (IORead(pin1) && !IORead(pin2)) { hardwareRevision = YUPIF4_RACE2; + } else if (!IORead(pin1) && !IORead(pin2)) { + hardwareRevision = YUPIF4_RACE3; } else if (!IORead(pin3)) { hardwareRevision = YUPIF4_NAV; } else { diff --git a/src/main/target/YUPIF4/hardware_revision.h b/src/main/target/YUPIF4/hardware_revision.h index 83d58c1602..7a167ea043 100644 --- a/src/main/target/YUPIF4/hardware_revision.h +++ b/src/main/target/YUPIF4/hardware_revision.h @@ -20,8 +20,9 @@ typedef enum yupif4HardwareRevision_t { UNKNOWN = 0, YUPIF4_RACE1, // Race V1 YUPIF4_RACE2, // Race V2 + YUPIF4_RACE3, // Race V3 YUPIF4_MINI, // Mini - YUPIF4_NAV, // Navigation + YUPIF4_NAV // Navigation } yupif4HardwareRevision_e; extern uint8_t hardwareRevision; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index cd0864dbab..8907c6423f 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -137,10 +137,12 @@ // ADC inputs #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC #define USE_ADC #define RSSI_ADC_GPIO_PIN PC0 #define VBAT_ADC_PIN PC1 #define CURRENT_METER_ADC_PIN PC2 +#define CURRENT_METER_SCALE_DEFAULT 150 // Default configuration #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL From db8698d8018212eda38eafa7339db3521cda7549 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 25 Sep 2017 05:15:45 +0100 Subject: [PATCH 128/138] Improved initialisation order. Stopped calling pidInit before gyro detected (#4218) --- src/main/fc/config.c | 69 ++++++++++++++++++++++++------------------- src/main/fc/fc_init.c | 53 +++++++++++++++------------------ src/main/flight/pid.c | 8 +++++ 3 files changed, 70 insertions(+), 60 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c8664976cc..820f9258c4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -267,7 +267,6 @@ static void setPidProfile(uint8_t pidProfileIndex) if (pidProfileIndex < MAX_PROFILE_COUNT) { systemConfigMutable()->pidProfileIndex = pidProfileIndex; currentPidProfile = pidProfilesMutable(pidProfileIndex); - pidInit(currentPidProfile); // re-initialise pid controller to re-initialise filters and config } } @@ -307,6 +306,7 @@ void activateConfig(void) resetAdjustmentStates(); + pidInit(currentPidProfile); useRcControlsConfig(currentPidProfile); useAdjustmentConfig(currentPidProfile); @@ -342,6 +342,13 @@ void validateAndFixConfig(void) #endif #ifndef USE_OSD_SLAVE + if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) { + systemConfigMutable()->activeRateProfile = 0; + } + if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) { + systemConfigMutable()->pidProfileIndex = 0; + } + if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)) { motorConfigMutable()->mincommand = 1000; } @@ -370,7 +377,7 @@ void validateAndFixConfig(void) if (featureConfigured(FEATURE_RX_SPI)) { featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); } -#endif +#endif // USE_RX_SPI if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); @@ -381,7 +388,7 @@ void validateAndFixConfig(void) if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; } -#endif +#endif // STM32F10X // software serial needs free PWM ports featureClear(FEATURE_SOFTSERIAL); } @@ -398,9 +405,9 @@ void validateAndFixConfig(void) if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; } -#endif +#endif // STM32F10X } -#endif +#endif // USE_SOFTSPI // Prevent invalid notch cutoff if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) { @@ -408,7 +415,7 @@ void validateAndFixConfig(void) } validateAndFixGyroConfig(); -#endif +#endif // USE_OSD_SLAVE if (!isSerialConfigValid(serialConfig())) { pgResetFn_serialConfig(serialConfigMutable()); @@ -501,6 +508,26 @@ void validateAndFixGyroConfig(void) gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } + + if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { + pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + gyroConfigMutable()->gyro_sync_denom = 1; + gyroConfigMutable()->gyro_use_32khz = false; + } + + if (gyroConfig()->gyro_use_32khz) { + // F1 and F3 can't handle high sample speed. +#if defined(STM32F1) + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); +#elif defined(STM32F3) + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); +#endif + } else { +#if defined(STM32F1) + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3); +#endif + } + float samplingTime; switch (gyroMpuDetectionResult()->sensor) { case ICM_20649_SPI: @@ -515,9 +542,6 @@ void validateAndFixGyroConfig(void) } if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { - pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - gyroConfigMutable()->gyro_sync_denom = 1; - gyroConfigMutable()->gyro_use_32khz = false; switch (gyroMpuDetectionResult()->sensor) { case ICM_20649_SPI: samplingTime = 1.0f / 1100.0f; @@ -530,20 +554,9 @@ void validateAndFixGyroConfig(void) if (gyroConfig()->gyro_use_32khz) { samplingTime = 0.00003125; - // F1 and F3 can't handle high sample speed. -#if defined(STM32F1) - gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); -#elif defined(STM32F3) - gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); -#endif - } else { -#if defined(STM32F1) - gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3); -#endif } // check for looptime restrictions based on motor protocol. Motor times have safety margin - const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; float motorUpdateRestriction; switch (motorConfig()->dev.motorPwmProtocol) { case (PWM_TYPE_STANDARD): @@ -568,6 +581,7 @@ void validateAndFixGyroConfig(void) } if (!motorConfig()->dev.useUnsyncedPwm) { + const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; if (pidLooptime < motorUpdateRestriction) { const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); @@ -594,19 +608,12 @@ void readEEPROM(void) if (!loadEEPROM()) { failureMode(FAILURE_INVALID_EEPROM_CONTENTS); } -#ifndef USE_OSD_SLAVE - if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {// sanity check - systemConfigMutable()->activeRateProfile = 0; - } - setControlRateProfile(systemConfig()->activeRateProfile); - - if (systemConfig()->pidProfileIndex >= MAX_PROFILE_COUNT) {// sanity check - systemConfigMutable()->pidProfileIndex = 0; - } - setPidProfile(systemConfig()->pidProfileIndex); -#endif validateAndFixConfig(); +#ifndef USE_OSD_SLAVE + setControlRateProfile(systemConfig()->activeRateProfile); + setPidProfile(systemConfig()->pidProfileIndex); +#endif activateConfig(); #ifndef USE_OSD_SLAVE diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 30bc288dca..ec8e011381 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -376,32 +376,6 @@ void init(void) serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif - mixerInit(mixerConfig()->mixerMode); -#ifdef USE_SERVOS - servosInit(); -#endif - - uint16_t idlePulse = motorConfig()->mincommand; - if (feature(FEATURE_3D)) { - idlePulse = flight3DConfig()->neutral3d; - } - - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { - featureClear(FEATURE_3D); - idlePulse = 0; // brushed motors - } - - mixerConfigureOutput(); - motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount()); - -#ifdef USE_SERVOS - servoConfigureOutput(); - if (isMixerUsingServos()) { - //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); - servoDevInit(&servoConfig()->dev); - } -#endif - if (0) {} #if defined(USE_PPM) else if (feature(FEATURE_RX_PPM)) { @@ -414,8 +388,6 @@ void init(void) } #endif - systemState |= SYSTEM_STATE_MOTORS_READY; - #ifdef BEEPER beeperInit(beeperDevConfig()); #endif @@ -528,10 +500,33 @@ void init(void) LED0_OFF; LED1_OFF; - // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit() + // gyro.targetLooptime set in sensorsAutodetect(), + // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter() + validateAndFixGyroConfig(); pidInit(currentPidProfile); + setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); + + mixerInit(mixerConfig()->mixerMode); + + uint16_t idlePulse = motorConfig()->mincommand; + if (feature(FEATURE_3D)) { + idlePulse = flight3DConfig()->neutral3d; + } + if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { + featureClear(FEATURE_3D); + idlePulse = 0; // brushed motors + } + mixerConfigureOutput(); + motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount()); + systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef USE_SERVOS + servosInit(); + servoConfigureOutput(); + if (isMixerUsingServos()) { + //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); + servoDevInit(&servoConfig()->dev); + } servosFilterInit(); #endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c8687ba162..a64d20dd9c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -170,6 +170,14 @@ void pidInitFilters(const pidProfile_t *pidProfile) { BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2 + if (targetPidLooptime == 0) { + // no looptime set, so set all the filters to null + dtermNotchFilterApplyFn = nullFilterApply; + dtermLpfApplyFn = nullFilterApply; + ptermYawFilterApplyFn = nullFilterApply; + return; + } + const uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed uint16_t dTermNotchHz; From e05b3cb4de6fded33b22b209e17b6ba1a5dae1ef Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 25 Sep 2017 04:55:53 +0100 Subject: [PATCH 129/138] Tidy of IO code --- src/main/drivers/io.c | 75 +++++++++++++++++++++++++++---------------- 1 file changed, 48 insertions(+), 27 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index ceb5090d2a..4f70032ff2 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -86,21 +86,22 @@ ioRec_t* IO_Rec(IO_t io) GPIO_TypeDef* IO_GPIO(IO_t io) { - ioRec_t *ioRec = IO_Rec(io); + const ioRec_t *ioRec = IO_Rec(io); return ioRec->gpio; } uint16_t IO_Pin(IO_t io) { - ioRec_t *ioRec = IO_Rec(io); + const ioRec_t *ioRec = IO_Rec(io); return ioRec->pin; } // port index, GPIOA == 0 int IO_GPIOPortIdx(IO_t io) { - if (!io) + if (!io) { return -1; + } return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10); // ports are 0x400 apart } @@ -117,8 +118,9 @@ int IO_GPIO_PortSource(IO_t io) // zero based pin index int IO_GPIOPinIdx(IO_t io) { - if (!io) + if (!io) { return -1; + } return 31 - __builtin_clz(IO_Pin(io)); // CLZ is a bit faster than FFS } @@ -135,8 +137,9 @@ int IO_GPIO_PinSource(IO_t io) // mask on stm32f103, bit index on stm32f303 uint32_t IO_EXTI_Line(IO_t io) { - if (!io) + if (!io) { return 0; + } #if defined(STM32F1) return 1 << IO_GPIOPinIdx(io); #elif defined(STM32F3) @@ -154,8 +157,9 @@ uint32_t IO_EXTI_Line(IO_t io) bool IORead(IO_t io) { - if (!io) + if (!io) { return false; + } #if defined(USE_HAL_DRIVER) return (LL_GPIO_ReadInputPort(IO_GPIO(io)) & IO_Pin(io)); #else @@ -165,8 +169,9 @@ bool IORead(IO_t io) void IOWrite(IO_t io, bool hi) { - if (!io) + if (!io) { return; + } #if defined(USE_HAL_DRIVER) LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io) << (hi ? 0 : 16)); #elif defined(STM32F4) @@ -183,8 +188,9 @@ void IOWrite(IO_t io, bool hi) void IOHi(IO_t io) { - if (!io) + if (!io) { return; + } #if defined(USE_HAL_DRIVER) LL_GPIO_SetOutputPin(IO_GPIO(io), IO_Pin(io)); #elif defined(STM32F4) @@ -196,8 +202,9 @@ void IOHi(IO_t io) void IOLo(IO_t io) { - if (!io) + if (!io) { return; + } #if defined(USE_HAL_DRIVER) LL_GPIO_ResetOutputPin(IO_GPIO(io), IO_Pin(io)); #elif defined(STM32F4) @@ -209,8 +216,9 @@ void IOLo(IO_t io) void IOToggle(IO_t io) { - if (!io) + if (!io) { return; + } uint32_t mask = IO_Pin(io); // Read pin state from ODR but write to BSRR because it only changes the pins @@ -238,8 +246,9 @@ void IOToggle(IO_t io) // claim IO pin, set owner and resources void IOInit(IO_t io, resourceOwner_e owner, uint8_t index) { - if (!io) + if (!io) { return; + } ioRec_t *ioRec = IO_Rec(io); ioRec->owner = owner; ioRec->index = index; @@ -247,17 +256,19 @@ void IOInit(IO_t io, resourceOwner_e owner, uint8_t index) void IORelease(IO_t io) { - if (!io) + if (!io) { return; + } ioRec_t *ioRec = IO_Rec(io); ioRec->owner = OWNER_FREE; } resourceOwner_e IOGetOwner(IO_t io) { - if (!io) + if (!io) { return OWNER_FREE; - ioRec_t *ioRec = IO_Rec(io); + } + const ioRec_t *ioRec = IO_Rec(io); return ioRec->owner; } @@ -265,9 +276,11 @@ resourceOwner_e IOGetOwner(IO_t io) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { - if (!io) + if (!io) { return; - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); GPIO_InitTypeDef init = { @@ -287,9 +300,11 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg) void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) { - if (!io) + if (!io) { return; - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); LL_GPIO_InitTypeDef init = { @@ -308,9 +323,11 @@ void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) void IOConfigGPIO(IO_t io, ioConfig_t cfg) { - if (!io) + if (!io) { return; - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + } + + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); GPIO_InitTypeDef init = { @@ -325,10 +342,11 @@ void IOConfigGPIO(IO_t io, ioConfig_t cfg) void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) { - if (!io) + if (!io) { return; + } - rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; + const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; RCC_ClockCmd(rcc, ENABLE); GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af); @@ -360,7 +378,8 @@ ioRec_t ioRecs[1]; // initialize all ioRec_t structures from ROM // currently only bitmask is used, this may change in future -void IOInitGlobal(void) { +void IOInitGlobal(void) +{ ioRec_t *ioRec = ioRecs; for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) { @@ -376,14 +395,16 @@ void IOInitGlobal(void) { IO_t IOGetByTag(ioTag_t tag) { - int portIdx = DEFIO_TAG_GPIOID(tag); - int pinIdx = DEFIO_TAG_PIN(tag); + const int portIdx = DEFIO_TAG_GPIOID(tag); + const int pinIdx = DEFIO_TAG_PIN(tag); - if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) + if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) { return NULL; + } // check if pin exists - if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) + if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) { return NULL; + } // count bits before this pin on single port int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]); // and add port offset From 33ecfb57359c4467488e03507388d719af3f5852 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 25 Sep 2017 12:12:27 +0100 Subject: [PATCH 130/138] Tidy of initialisation (#4229) --- src/main/fc/config.c | 34 +++++++++++++++------------------- 1 file changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 820f9258c4..2e67640b51 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -508,7 +508,6 @@ void validateAndFixGyroConfig(void) gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } - if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed gyroConfigMutable()->gyro_sync_denom = 1; @@ -540,7 +539,6 @@ void validateAndFixGyroConfig(void) samplingTime = 0.000125f; break; } - if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { switch (gyroMpuDetectionResult()->sensor) { case ICM_20649_SPI: @@ -551,7 +549,6 @@ void validateAndFixGyroConfig(void) break; } } - if (gyroConfig()->gyro_use_32khz) { samplingTime = 0.00003125; } @@ -559,41 +556,40 @@ void validateAndFixGyroConfig(void) // check for looptime restrictions based on motor protocol. Motor times have safety margin float motorUpdateRestriction; switch (motorConfig()->dev.motorPwmProtocol) { - case (PWM_TYPE_STANDARD): + case PWM_TYPE_STANDARD: motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE; break; - case (PWM_TYPE_ONESHOT125): + case PWM_TYPE_ONESHOT125: motorUpdateRestriction = 0.0005f; break; - case (PWM_TYPE_ONESHOT42): + case PWM_TYPE_ONESHOT42: motorUpdateRestriction = 0.0001f; break; #ifdef USE_DSHOT - case (PWM_TYPE_DSHOT150): + case PWM_TYPE_DSHOT150: motorUpdateRestriction = 0.000250f; break; - case (PWM_TYPE_DSHOT300): + case PWM_TYPE_DSHOT300: motorUpdateRestriction = 0.0001f; break; #endif - default: - motorUpdateRestriction = 0.00003125f; + default: + motorUpdateRestriction = 0.00003125f; + break; } - if (!motorConfig()->dev.useUnsyncedPwm) { - const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; - if (pidLooptime < motorUpdateRestriction) { - const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); - - pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom); - } - } else { + if (motorConfig()->dev.useUnsyncedPwm) { // Prevent overriding the max rate of motors if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) { const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); - motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate); } + } else { + const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom; + if (pidLooptime < motorUpdateRestriction) { + const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM); + pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom); + } } } #endif From cc573574b2f070d270926c157827101a05010636 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 24 Sep 2017 22:10:11 +0900 Subject: [PATCH 131/138] Preinit for MAX7456 on Kakute F4 requires output hi. --- src/main/drivers/bus_spi.h | 2 ++ src/main/drivers/bus_spi_config.c | 19 ++++++++++++++++++- src/main/fc/fc_init.c | 2 +- 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 88507bc10f..a062c44ee4 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -87,6 +87,8 @@ typedef enum SPIDevice { #define SPI_DEV_TO_CFG(x) ((x) + 1) void spiPreInitCs(ioTag_t iotag); +void spiPreInitCsOutPU(ioTag_t iotag); + bool spiInit(SPIDevice device); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data); diff --git a/src/main/drivers/bus_spi_config.c b/src/main/drivers/bus_spi_config.c index 68643797ff..897b78e254 100644 --- a/src/main/drivers/bus_spi_config.c +++ b/src/main/drivers/bus_spi_config.c @@ -25,7 +25,14 @@ // Bring a pin for possible CS line to pull-up state in preparation for // sequential initialization by relevant drivers. -// Note that the pin is set to input for safety at this point. + +// There are two versions: +// spiPreInitCs set the pin to input with pullup (IOCFG_IPU) for safety at this point. +// spiPreInitCsOutPU which actually drive the pin for digital hi. +// +// The later is required for SPI slave devices on some targets, interfaced through level shifters, such as Kakute F4. +// Note that with this handling, a pin declared as CS pin for MAX7456 needs special care when re-purposing the pin for other, especially, input uses. +// This will/should be fixed when we go fully reconfigurable. void spiPreInitCs(ioTag_t iotag) { @@ -35,3 +42,13 @@ void spiPreInitCs(ioTag_t iotag) IOConfigGPIO(io, IOCFG_IPU); } } + +void spiPreInitCsOutPU(ioTag_t iotag) +{ + IO_t io = IOGetByTag(iotag); + if (io) { + IOInit(io, OWNER_SPI_PREINIT, 0); + IOConfigGPIO(io, IOCFG_OUT_PP); + IOHi(io); + } +} diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 30bc288dca..97301217e4 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -213,7 +213,7 @@ void spiPreInit(void) spiPreInitCs(IO_TAG(L3GD20_CS_PIN)); #endif #ifdef USE_MAX7456 - spiPreInitCs(IO_TAG(MAX7456_SPI_CS_PIN)); + spiPreInitCsOutPU(IO_TAG(MAX7456_SPI_CS_PIN)); // XXX 3.2 workaround for Kakute F4. See comment for spiPreInitCSOutPU. #endif #ifdef USE_SDCARD spiPreInitCs(IO_TAG(SDCARD_SPI_CS_PIN)); From cbefe71a738a72b7ceeaa0d334b3d545b638b12e Mon Sep 17 00:00:00 2001 From: jirif Date: Tue, 26 Sep 2017 12:36:54 +0200 Subject: [PATCH 132/138] Fast stick commands (#4208) Implemented time-based stick command handling --- src/main/fc/rc_controls.c | 86 ++++++++++++++------------- src/test/unit/rc_controls_unittest.cc | 3 + 2 files changed, 49 insertions(+), 40 deletions(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index b2dcf762ad..475de617c4 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -57,6 +57,7 @@ #include "sensors/acceleration.h" #include "rx/rx.h" +#include "scheduler/scheduler.h" #include "flight/pid.h" #include "flight/navigation.h" @@ -118,11 +119,22 @@ throttleStatus_e calculateThrottleStatus(void) return THROTTLE_HIGH; } +#define ARM_DELAY_MS 500 +#define STICK_DELAY_MS 50 +#define STICK_AUTOREPEAT_MS 250 +#define repeatAfter(t) { \ + rcDelayMs -= (t); \ + doNotRepeat = false; \ +} void processRcStickPositions(throttleStatus_e throttleStatus) { - static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors - static uint8_t rcSticks; // this hold sticks position for command combos - static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it + // time the sticks are maintained + static int16_t rcDelayMs; + // hold sticks position for command combos + static uint8_t rcSticks; + // an extra guard for disarming through switch to prevent that one frame can disarm it + static uint8_t rcDisarmTicks; + static bool doNotRepeat; uint8_t stTmp = 0; int i; @@ -132,7 +144,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } #endif - // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; @@ -142,15 +153,16 @@ void processRcStickPositions(throttleStatus_e throttleStatus) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { - if (rcDelayCommand < 250) - rcDelayCommand++; - } else - rcDelayCommand = 0; + if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) + rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000; + } else { + rcDelayMs = 0; + doNotRepeat = false; + } rcSticks = stTmp; // perform actions if (!isUsingSticksToArm) { - if (IS_RC_MODE_ACTIVE(BOXARM)) { rcDisarmTicks = 0; // Arming via ARM BOX @@ -158,7 +170,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } else { // Disarming via ARM BOX resetArmingDisabled(); - if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { @@ -170,31 +181,37 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } } } - } - - if (rcDelayCommand != 20) { - return; - } - - if (isUsingSticksToArm) { - // Disarm on throttle down + yaw - if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { + } else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { + if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { + doNotRepeat = true; + // Disarm on throttle down + yaw if (ARMING_FLAG(ARMED)) disarm(); else { - beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held - rcDelayCommand = 0; // reset so disarm tone will repeat + beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held + repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat + } + } + return; + } else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { + if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { + doNotRepeat = true; + if (!ARMING_FLAG(ARMED)) { + // Arm via YAW + tryArm(); + } else { + resetArmingDisabled(); } } - } - - if (ARMING_FLAG(ARMED)) { - // actions during armed return; } + if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) { + return; + } + doNotRepeat = true; + // actions during not armed - i = 0; if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration @@ -221,6 +238,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } // Multiple configuration profiles + i = 0; if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 @@ -236,18 +254,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) saveConfigAndNotify(); } - if (isUsingSticksToArm) { - - if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { - // Arm via YAW - tryArm(); - - return; - } else { - resetArmingDisabled(); - } - } - if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); @@ -283,7 +289,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } if (shouldApplyRollAndPitchTrimDelta) { applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta); - rcDelayCommand = 0; // allow autorepetition + repeatAfter(STICK_AUTOREPEAT_MS); return; } @@ -325,7 +331,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0); } else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) { cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000); - } + } #endif } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 1e85421cdc..47481adffe 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -48,6 +48,8 @@ extern "C" { #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" + + #include "scheduler/scheduler.h" } #include "unittest_macros.h" @@ -703,4 +705,5 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); void resetArmingDisabled(void) {} +timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 20000; } } From de4c11ab6ca44eabf9bb3631441ace01bb4b93d2 Mon Sep 17 00:00:00 2001 From: Faduf Date: Tue, 26 Sep 2017 18:41:20 +0200 Subject: [PATCH 133/138] Add UART1 inverter to YupiF4 target --- src/main/target/YUPIF4/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 8907c6423f..0d91ddf32c 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -34,8 +34,6 @@ #define BEEPER_OPT PB14 #define BEEPER_PWM_HZ 3150 // Beeper PWM frequency in Hz -#define INVERTER_PIN_UART6 PB15 - // Gyro interrupt #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL @@ -72,6 +70,7 @@ // UART Ports #define USE_UART1 +#define INVERTER_PIN_UART1 PB12 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 @@ -80,6 +79,7 @@ #define UART3_TX_PIN PB10 #define USE_UART6 +#define INVERTER_PIN_UART6 PB15 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 From f564cdf9525e9801293d6324dec224b711738117 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Wed, 27 Sep 2017 14:12:37 +0200 Subject: [PATCH 134/138] Fix atomic in unittests (#4240) Add unittest support for ATOMIC_BLOCK - BASEPRI emulation on UNIT_TEST target - documentation cleanup / fixed missspelings - ATOMIC_BARRIER implemented for CLang, using blocks - ATOMIC_BARRIER is using macros to separate barriers gcc specific unittest for ATOMIC_BARRIER ATOMIC_BARRIER clang unittest quick hack to enable CLangs -flocks + ATOMIC_BARRIER tests Needs cleanup * Enable test * Add libBlocksRuntime dependency to Travis --- .travis.yml | 1 + src/main/build/atomic.c | 7 ++ src/main/build/atomic.h | 119 +++++++++++++++++++++++++----- src/test/Makefile | 16 +++- src/test/unit/atomic_unittest.cc | 116 +++++++++++++++++++++++++++++ src/test/unit/atomic_unittest_c.c | 37 ++++++++++ 6 files changed, 274 insertions(+), 22 deletions(-) create mode 100644 src/main/build/atomic.c create mode 100644 src/test/unit/atomic_unittest.cc create mode 100644 src/test/unit/atomic_unittest_c.c diff --git a/.travis.yml b/.travis.yml index 82f041a159..7731ac2a5e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,6 +32,7 @@ addons: - git - libc6-i386 - time + - libblocksruntime-dev # We use cpp for unit tests, and c for the main project. language: cpp diff --git a/src/main/build/atomic.c b/src/main/build/atomic.c new file mode 100644 index 0000000000..540aacb2ad --- /dev/null +++ b/src/main/build/atomic.c @@ -0,0 +1,7 @@ +#include + +#include "atomic.h" + +#if defined(UNIT_TEST) +uint8_t atomic_BASEPRI; +#endif diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 63d2e78ffe..e18d632b86 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -17,76 +17,133 @@ #pragma once -// only set_BASEPRI is implemented in device library. It does always create memory barrirer +#include + +#if !defined(UNIT_TEST) +// BASEPRI manipulation functions +// only set_BASEPRI is implemented in device library. It does always create memory barrier // missing versions are implemented here -// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier +// set BASEPRI register, do not create memory barrier __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri) { __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) ); } +// set BASEPRI_MAX register, do not create memory barrier __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri) { __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) ); } -#if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */ +// set BASEPRI_MAX register, with memory barrier +# if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri) { __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" ); } +# endif + #endif -// cleanup BASEPRI restore function, with global memory barrier +#if defined(UNIT_TEST) +// atomic related functions for unittest. + +extern uint8_t atomic_BASEPRI; + +static inline uint8_t __get_BASEPRI(void) +{ + return atomic_BASEPRI; +} + +// restore BASEPRI (called as cleanup function), with global memory barrier +static inline void __basepriRestoreMem(uint8_t *val) +{ + atomic_BASEPRI = *val; + asm volatile ("": : :"memory"); // compiler memory barrier +} + +// increase BASEPRI, with global memory barrier, returns true +static inline uint8_t __basepriSetMemRetVal(uint8_t prio) +{ + if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) { + atomic_BASEPRI = prio; + } + asm volatile ("": : :"memory"); // compiler memory barrier + return 1; +} + +// restore BASEPRI (called as cleanup function), no memory barrier +static inline void __basepriRestore(uint8_t *val) +{ + atomic_BASEPRI = *val; +} + +// increase BASEPRI, no memory barrier, returns true +static inline uint8_t __basepriSetRetVal(uint8_t prio) +{ + if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) { + atomic_BASEPRI = prio; + } + return 1; +} + +#else +// ARM BASEPRI manipulation + +// restore BASEPRI (called as cleanup function), with global memory barrier static inline void __basepriRestoreMem(uint8_t *val) { __set_BASEPRI(*val); } -// set BASEPRI_MAX function, with global memory barrier, returns true +// set BASEPRI_MAX, with global memory barrier, returns true static inline uint8_t __basepriSetMemRetVal(uint8_t prio) { __set_BASEPRI_MAX(prio); return 1; } -// cleanup BASEPRI restore function, no memory barrier +// restore BASEPRI (called as cleanup function), no memory barrier static inline void __basepriRestore(uint8_t *val) { __set_BASEPRI_nb(*val); } -// set BASEPRI_MAX function, no memory barrier, returns true +// set BASEPRI_MAX, no memory barrier, returns true static inline uint8_t __basepriSetRetVal(uint8_t prio) { __set_BASEPRI_MAX_nb(prio); return 1; } -// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled -// Full memory barrier is placed at start and exit of block -#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \ +#endif + +// Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. +// All exit paths are handled. Implemented as for loop, does intercept break and continue +// Full memory barrier is placed at start and at exit of block +// __unused__ attribute is used to supress CLang warning +#define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__ ((__cleanup__ (__basepriRestoreMem), __unused__)) = __get_BASEPRI(), \ __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 ) // Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create memory barrier. -// Be careful when using this, you must use some method to prevent optimizer form breaking things +// Be careful when using this, you must use some method to prevent optimizer from breaking things // - lto is used for Cleanflight compilation, so function call is not memory barrier // - use ATOMIC_BARRIER or volatile to protect used variables // - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much // - gcc 5 and later works as intended, generating quite optimal code -#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \ +#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__ ((__cleanup__ (__basepriRestore), __unused__)) = __get_BASEPRI(), \ __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \ // ATOMIC_BARRIER // Create memory barrier // - at the beginning of containing block (value of parameter must be reread from memory) // - at exit of block (all exit paths) (parameter value if written into memory, but may be cached in register for subsequent use) -// On gcc 5 and higher, this protects only memory passed as parameter (any type should work) +// On gcc 5 and higher, this protects only memory passed as parameter (any type can be used) // this macro can be used only ONCE PER LINE, but multiple uses per block are fine #if (__GNUC__ > 6) -#warning "Please verify that ATOMIC_BARRIER works as intended" +# warning "Please verify that ATOMIC_BARRIER works as intended" // increment version number if BARRIER works // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead // you should check that local variable scope with cleanup spans entire block @@ -98,15 +155,37 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) # define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__) #endif -// this macro uses local function for cleanup. CLang block can be substituded +#define ATOMIC_BARRIER_ENTER(dataPtr, refStr) \ + __asm__ volatile ("\t# barrier (" refStr ") enter\n" : "+m" (*(dataPtr))) + +#define ATOMIC_BARRIER_LEAVE(dataPtr, refStr) \ + __asm__ volatile ("\t# barrier (" refStr ") leave\n" : "m" (*(dataPtr))) + +#if defined(__clang__) +// CLang version, using Objective C-style block +// based on https://stackoverflow.com/questions/24959440/rewrite-gcc-cleanup-macro-with-nested-function-for-clang +typedef void (^__cleanup_block)(void); +static inline void __do_cleanup(__cleanup_block * b) { (*b)(); } + +#define ATOMIC_BARRIER(data) \ + typeof(data) *__UNIQL(__barrier) = &data; \ + ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data); \ + __cleanup_block __attribute__((cleanup(__do_cleanup) __unused__)) __UNIQL(__cleanup) = \ + ^{ ATOMIC_BARRIER_LEAVE(__UNIQL(__barrier), #data); }; \ + do {} while(0) \ +/**/ +#else +// gcc version, uses local function for cleanup. #define ATOMIC_BARRIER(data) \ __extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \ - __asm__ volatile ("\t# barrier(" #data ") end\n" : : "m" (**__d)); \ + ATOMIC_BARRIER_LEAVE(*__d, #data); \ } \ - typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ - __asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier))) + typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ + ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data); \ + do {} while(0) \ +/**/ +#endif - -// define these wrappers for atomic operations, use gcc buildins +// define these wrappers for atomic operations, using gcc builtins #define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val) #define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val) diff --git a/src/test/Makefile b/src/test/Makefile index 96562f9eb7..56b1965178 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -35,6 +35,9 @@ arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/common/bitarray.c +atomic_unittest_SRC = \ + $(USER_DIR)/build/atomic.c \ + $(TEST_DIR)/atomic_unittest_c.c baro_bmp085_unittest_SRC := \ $(USER_DIR)/drivers/barometer/barometer_bmp085.c \ @@ -299,6 +302,8 @@ UNAME := $(shell uname -s) # Use clang/clang++ by default CC := clang CXX := clang++ +#CC := gcc +#CXX := g++ COMMON_FLAGS = \ -g \ @@ -308,6 +313,7 @@ COMMON_FLAGS = \ -ggdb3 \ -O0 \ -DUNIT_TEST \ + -fblocks \ -isystem $(GTEST_DIR)/inc \ -MMD -MP @@ -432,7 +438,7 @@ TEST_CFLAGS = $(addprefix -I,$(TEST_INCLUDE_DIRS)) # param $1 = testname define test-specific-stuff -$$1_OBJS = $$(patsubst $$(USER_DIR)%,$$(OBJECT_DIR)/$1%,$$($1_SRC:=.o)) +$$1_OBJS = $$(patsubst $$(TEST_DIR)%,$$(OBJECT_DIR)/$1%, $$(patsubst $$(USER_DIR)%,$$(OBJECT_DIR)/$1%,$$($1_SRC:=.o))) # $$(info $1 -v-v-------) # $$(info $1_SRC: $($1_SRC)) @@ -452,6 +458,12 @@ $(OBJECT_DIR)/$1/%.c.o: $(USER_DIR)/%.c $(foreach def,$($1_DEFINES),-D $(def)) \ -c $$< -o $$@ +$(OBJECT_DIR)/$1/%.c.o: $(TEST_DIR)/%.c + @echo "compiling test c file: $$<" "$(STDOUT)" + $(V1) mkdir -p $$(dir $$@) + $(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \ + $(foreach def,$($1_DEFINES),-D $(def)) \ + -c $$< -o $$@ $(OBJECT_DIR)/$1/$1.o: $(TEST_DIR)/$1.cc @echo "compiling $$<" "$(STDOUT)" @@ -467,7 +479,7 @@ $(OBJECT_DIR)/$1/$1 : $$($$1_OBJS) \ @echo "linking $$@" "$(STDOUT)" $(V1) mkdir -p $(dir $$@) - $(V1) $(CXX) $(CXX_FLAGS) $(PG_FLAGS) $$^ -o $$@ + $(V1) $(CXX) $(CXX_FLAGS) $(PG_FLAGS) -lBlocksRuntime $$^ -o $$@ test_$1: $(OBJECT_DIR)/$1/$1 $(V1) $$< $$(EXEC_OPTS) "$(STDOUT)" && echo "running $$@: PASS" diff --git a/src/test/unit/atomic_unittest.cc b/src/test/unit/atomic_unittest.cc new file mode 100644 index 0000000000..17f9275bc8 --- /dev/null +++ b/src/test/unit/atomic_unittest.cc @@ -0,0 +1,116 @@ +/* + * 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 "unittest_macros.h" +#include "gtest/gtest.h" + +extern "C" { +#include "build/atomic.h" +} + +TEST(AtomicUnittest, TestAtomicBlock) +{ + atomic_BASEPRI = 0; // reset BASEPRI + + ATOMIC_BLOCK(10) { + EXPECT_EQ(atomic_BASEPRI, 10); // locked + ATOMIC_BLOCK(5) { + EXPECT_EQ(atomic_BASEPRI, 5); // priority increase + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + ATOMIC_BLOCK(20) { + EXPECT_EQ(atomic_BASEPRI, 10); // lower priority, no change to value + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + } + EXPECT_EQ(atomic_BASEPRI, 0); // restore priority to unlocked +} + +TEST(AtomicUnittest, TestAtomicBlockNB) +{ + atomic_BASEPRI = 0; // reset BASEPRI + + ATOMIC_BLOCK_NB(10) { + EXPECT_EQ(atomic_BASEPRI, 10); // locked + ATOMIC_BLOCK_NB(5) { + EXPECT_EQ(atomic_BASEPRI, 5); // priority increase + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + ATOMIC_BLOCK_NB(20) { + EXPECT_EQ(atomic_BASEPRI, 10); // lower priority, no change to value + } + EXPECT_EQ(atomic_BASEPRI, 10); // restore priority + } + EXPECT_EQ(atomic_BASEPRI, 0); // restore priority to unlocked +} + +#if 1 // not working now ... (CLang needs -fblock + libraries / some unittests don't support gcc) + +struct barrierTrace { + int enter, leave; +}; + +extern "C" { + int testAtomicBarrier_C(struct barrierTrace *b0, struct barrierTrace *b1, struct barrierTrace sample[][2]); +} + +TEST(AtomicUnittest, TestAtomicBarrier) +{ + barrierTrace b0,b1,sample[10][2]; + + int sampled = testAtomicBarrier_C(&b0,&b1,sample); + int sIdx = 0; + EXPECT_EQ(sample[sIdx][0].enter, 0); + EXPECT_EQ(sample[sIdx][0].leave, 0); + EXPECT_EQ(sample[sIdx][1].enter, 0); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + // do { + // ATOMIC_BARRIER(*b0); + // ATOMIC_BARRIER(*b1); + EXPECT_EQ(sample[sIdx][0].enter, 1); + EXPECT_EQ(sample[sIdx][0].leave, 0); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + // do { + // ATOMIC_BARRIER(*b0); + EXPECT_EQ(sample[sIdx][0].enter, 2); + EXPECT_EQ(sample[sIdx][0].leave, 0); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + // } while(0); + EXPECT_EQ(sample[sIdx][0].enter, 2); + EXPECT_EQ(sample[sIdx][0].leave, 1); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 0); + sIdx++; + //} while(0); + EXPECT_EQ(sample[sIdx][0].enter, 2); + EXPECT_EQ(sample[sIdx][0].leave, 2); + EXPECT_EQ(sample[sIdx][1].enter, 1); + EXPECT_EQ(sample[sIdx][1].leave, 1); + sIdx++; + //return sIdx; + EXPECT_EQ(sIdx, sampled); +} + +#endif diff --git a/src/test/unit/atomic_unittest_c.c b/src/test/unit/atomic_unittest_c.c new file mode 100644 index 0000000000..a502e0de46 --- /dev/null +++ b/src/test/unit/atomic_unittest_c.c @@ -0,0 +1,37 @@ +#include "build/atomic.h" + +struct barrierTrace { + int enter, leave; +}; + +int testAtomicBarrier_C(struct barrierTrace *b0, struct barrierTrace *b1, struct barrierTrace sample[][2]) +{ + int sIdx = 0; + +// replace barrier macros to track barrier invocation +// pass known struct as barrier variable, keep track inside it +#undef ATOMIC_BARRIER_ENTER +#undef ATOMIC_BARRIER_LEAVE +#define ATOMIC_BARRIER_ENTER(ptr, refStr) do {(ptr)->enter++; } while(0) +#define ATOMIC_BARRIER_LEAVE(ptr, refStr) do {(ptr)->leave++; } while(0) + + b0->enter = 0; b0->leave = 0; + b1->enter = 0; b1->leave = 0; + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + do { + ATOMIC_BARRIER(*b0); + ATOMIC_BARRIER(*b1); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + do { + ATOMIC_BARRIER(*b0); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + } while(0); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + } while(0); + sample[sIdx][0]=*b0; sample[sIdx][1]=*b1; sIdx++; + return sIdx; + +// ATOMIC_BARRIER is broken in rest of this file +#undef ATOMIC_BARRIER_ENTER +#undef ATOMIC_BARRIER_LEAVE +} From acedb4676d0f69ea52bd71fedcc9092b5e92d1a9 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Wed, 27 Sep 2017 14:24:58 +0200 Subject: [PATCH 135/138] Add -flocks only to clang --- src/test/Makefile | 16 ++++++++++------ src/test/unit/atomic_unittest.cc | 4 ---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 56b1965178..d89c1aa725 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -11,6 +11,7 @@ # Where to find user code. USER_DIR = ../main +TEST_DIR = unit # specify which files that are included in the test in addition to the unittest file. @@ -35,7 +36,7 @@ arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/runtime_config.c \ $(USER_DIR)/common/bitarray.c -atomic_unittest_SRC = \ +atomic_unittest_SRC := \ $(USER_DIR)/build/atomic.c \ $(TEST_DIR)/atomic_unittest_c.c @@ -291,7 +292,6 @@ ringbuffer_unittest_SRC := \ GTEST_DIR = ../../lib/test/gtest -TEST_DIR = unit USER_INCLUDE_DIR = $(USER_DIR) OBJECT_DIR = ../../obj/test @@ -313,10 +313,14 @@ COMMON_FLAGS = \ -ggdb3 \ -O0 \ -DUNIT_TEST \ - -fblocks \ -isystem $(GTEST_DIR)/inc \ -MMD -MP +ifeq ($(shell $(CC) -v 2>&1 | grep -q "clang version" && echo "clang"),clang) +COMMON_FLAGS += -fblocks +LDFLAGS += -lBlocksRuntime +endif + ifneq ($(UNAME), Darwin) COMMON_FLAGS += -pthread endif @@ -337,9 +341,9 @@ CXX_FLAGS += $(COVERAGE_FLAGS) # Set up the parameter group linker flags according to OS ifeq ($(UNAME), Darwin) -PG_FLAGS = -Wl,-map,$(OBJECT_DIR)/$@.map +LDFLAGS += -Wl,-map,$(OBJECT_DIR)/$@.map else -PG_FLAGS = -Wl,-T,$(TEST_DIR)/parameter_group.ld -Wl,-Map,$(OBJECT_DIR)/$@.map +LDFLAGS += -Wl,-T,$(TEST_DIR)/parameter_group.ld -Wl,-Map,$(OBJECT_DIR)/$@.map endif # Gather up all of the tests. @@ -479,7 +483,7 @@ $(OBJECT_DIR)/$1/$1 : $$($$1_OBJS) \ @echo "linking $$@" "$(STDOUT)" $(V1) mkdir -p $(dir $$@) - $(V1) $(CXX) $(CXX_FLAGS) $(PG_FLAGS) -lBlocksRuntime $$^ -o $$@ + $(V1) $(CXX) $(CXX_FLAGS) $(LDFLAGS) $$^ -o $$@ test_$1: $(OBJECT_DIR)/$1/$1 $(V1) $$< $$(EXEC_OPTS) "$(STDOUT)" && echo "running $$@: PASS" diff --git a/src/test/unit/atomic_unittest.cc b/src/test/unit/atomic_unittest.cc index 17f9275bc8..39c4bf23a8 100644 --- a/src/test/unit/atomic_unittest.cc +++ b/src/test/unit/atomic_unittest.cc @@ -61,8 +61,6 @@ TEST(AtomicUnittest, TestAtomicBlockNB) EXPECT_EQ(atomic_BASEPRI, 0); // restore priority to unlocked } -#if 1 // not working now ... (CLang needs -fblock + libraries / some unittests don't support gcc) - struct barrierTrace { int enter, leave; }; @@ -112,5 +110,3 @@ TEST(AtomicUnittest, TestAtomicBarrier) //return sIdx; EXPECT_EQ(sIdx, sampled); } - -#endif From 5d5a5728235d7a3465ae9341a9cf540106beb057 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Wed, 27 Sep 2017 14:37:29 +0200 Subject: [PATCH 136/138] Update README.md Fix typos in my name --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a5f0b30909..fc04392bf1 100644 --- a/README.md +++ b/README.md @@ -101,7 +101,7 @@ Big thanks to current and past contributors: * Blackman, Jason (blckmn) * ctzsnooze * Höglund, Anders (andershoglund) -* Ledvin, Peter (ledvinap) - **IO code awesomeness!** +* Ledvina, Petr (ledvinap) - **IO code awesomeness!** * kc10kevin * Keeble, Gary (MadmanK) * Keller, Michael (mikeller) - **Configurator brilliance** From 28162447ed34935e0b9202e887de414605d88f60 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 27 Sep 2017 18:19:41 +0200 Subject: [PATCH 137/138] Update Vagrant env with libblocksruntime-dev required by unittest. --- Vagrantfile | 1 + 1 file changed, 1 insertion(+) diff --git a/Vagrantfile b/Vagrantfile index f21fcf0a90..5188db64cc 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -31,6 +31,7 @@ Vagrant.configure(2) do |config| apt-get update apt-get install -y git gcc-arm-embedded=6-2017q2-1~xenial1 apt-get install -y make python gcc clang + apt-get install -y libblocksruntime-dev SHELL end From f9724c8d774137b7a5a9436f956d6020dec05f20 Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 28 Sep 2017 22:43:12 +0900 Subject: [PATCH 138/138] Beeper is on PB4, and is INVERTED --- src/main/target/BETAFLIGHTF4/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index a17bb56086..054246b87a 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -24,8 +24,8 @@ #define LED0_PIN PB5 -// Leave beeper here but with none as io - so disabled unless mapped. #define BEEPER PB4 +#define BEEPER_INVERTED // PC13 used as inverter select GPIO for UART2 #define INVERTER_PIN_UART2 PC13