From f32580601a140d45c289c198b8afa1e847af285a Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 27 Jan 2017 00:16:05 +1300 Subject: [PATCH 01/55] Fix for problem with CLI 'diff' in the case of nonexistent default values. --- src/main/fc/cli.c | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d233d3852..2d2dddde4 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1150,7 +1150,7 @@ static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxCo for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; const rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultRxConfig) { channelFailsafeConfigurationDefault = &defaultRxConfig->failsafe_channel_configurations[channel]; equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode @@ -1273,7 +1273,7 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva // print out aux channel settings for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = &modeActivationProfile->modeActivationConditions[i]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultModeActivationProfile) { const modeActivationCondition_t *macDefault = &defaultModeActivationProfile->modeActivationConditions[i]; equalsDefault = mac->modeId == macDefault->modeId @@ -1345,7 +1345,7 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { continue; }; - bool equalsDefault = true; + bool equalsDefault = false; if (serialConfigDefault) { equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask @@ -1540,7 +1540,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad // print out adjustment ranges channel settings for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { const adjustmentRange_t *ar = &adjustmentProfile->adjustmentRanges[i]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultAdjustmentProfile) { const adjustmentRange_t *arDefault = &defaultAdjustmentProfile->adjustmentRanges[i]; equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex @@ -1646,7 +1646,7 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer const float roll = customMotorMixer[i].roll; const float pitch = customMotorMixer[i].pitch; const float yaw = customMotorMixer[i].yaw; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultCustomMotorMixer) { const float thrDefault = defaultCustomMotorMixer[i].throttle; const float rollDefault = defaultCustomMotorMixer[i].roll; @@ -1744,7 +1744,7 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC const char *format = "rxrange %u %u %u\r\n"; for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { const rxChannelRangeConfiguration_t *channelRangeConfiguration = &rxConfig->channelRanges[i]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultRxConfig) { const rxChannelRangeConfiguration_t *channelRangeConfigurationDefault = &defaultRxConfig->channelRanges[i]; equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min @@ -1814,7 +1814,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t ledConfig = ledConfigs[i]; generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); - bool equalsDefault = true; + bool equalsDefault = false; if (defaultLedConfigs) { ledConfig_t ledConfigDefault = defaultLedConfigs[i]; equalsDefault = ledConfig == ledConfigDefault; @@ -1851,7 +1851,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo const char *format = "color %u %d,%u,%u\r\n"; for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { const hsvColor_t *color = &colors[i]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultColors) { const hsvColor_t *colorDefault = &defaultColors[i]; equalsDefault = color->h == colorDefault->h @@ -1890,7 +1890,7 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { int colorIndex = ledStripConfig->modeColors[i].color[j]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultLedStripConfig) { int colorIndexDefault = defaultLedStripConfig->modeColors[i].color[j]; equalsDefault = colorIndex == colorIndexDefault; @@ -1902,7 +1902,7 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { const int colorIndex = ledStripConfig->specialColors.color[j]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultLedStripConfig) { const int colorIndexDefault = defaultLedStripConfig->specialColors.color[j]; equalsDefault = colorIndex == colorIndexDefault; @@ -1912,7 +1912,7 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon } const int ledStripAuxChannel = ledStripConfig->ledstrip_aux_channel; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultLedStripConfig) { const int ledStripAuxChannelDefault = defaultLedStripConfig->ledstrip_aux_channel; equalsDefault = ledStripAuxChannel == ledStripAuxChannelDefault; @@ -1960,7 +1960,7 @@ static void printServo(uint8_t dumpMask, servoProfile_t *defaultServoProfile) const char *format = "servo %u %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { const servoParam_t *servoConf = &servoProfile()->servoConf[i]; - bool equalsDefault = true; + bool equalsDefault = false; if (defaultServoProfile) { const servoParam_t *servoConfDefault = &defaultServoProfile->servoConf[i]; equalsDefault = servoConf->min == servoConfDefault->min @@ -2080,7 +2080,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) break; } - bool equalsDefault = true; + bool equalsDefault = false; if (defaultConfig) { servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel @@ -2428,7 +2428,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig) { // print out vtx channel settings const char *format = "vtx %u %u %u %u %u %u\r\n"; - bool equalsDefault = true; + bool equalsDefault = false; for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { const vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; if (defaultConfig) { @@ -2702,7 +2702,7 @@ static void cliBeeper(char *cmdline) static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) { - bool equalsDefault = true; + bool equalsDefault = false; char buf[16]; char bufDefault[16]; uint32_t i; From 94993e55c072263c13d397df0ae7af572841f047 Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 26 Jan 2017 00:51:51 +1300 Subject: [PATCH 02/55] Fixed iBus telemetry handling (cherry-pick from @marcroe 's work in #2170. --- src/main/fc/config.c | 3 +++ src/main/target/common.h | 4 ++-- src/main/telemetry/ibus.c | 4 ++-- src/main/telemetry/telemetry.c | 6 ++++++ 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 22b28d1f6..a4cebaef3 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -407,6 +407,9 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) telemetryConfig->frsky_vfas_cell_voltage = 0; telemetryConfig->hottAlarmSoundInterval = 5; telemetryConfig->pidValuesAsTelemetry = 0; +#ifdef TELEMETRY_IBUS + telemetryConfig->report_cell_voltage = false; +#endif } #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index a6db3d021..1a1a9ac57 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -34,6 +34,7 @@ #define STM_FAST_TARGET #define I2C3_OVERCLOCK true #define I2C4_OVERCLOCK true +#define TELEMETRY_IBUS #endif /**************************** @@ -43,6 +44,7 @@ #define STM_FAST_TARGET #define USE_DSHOT #define I2C3_OVERCLOCK true +#define TELEMETRY_IBUS #endif #ifdef STM32F3 @@ -85,7 +87,6 @@ #define TELEMETRY #define TELEMETRY_FRSKY #define TELEMETRY_HOTT -#define TELEMETRY_IBUS #define TELEMETRY_LTM #define TELEMETRY_SMARTPORT #define USE_SERVOS @@ -100,7 +101,6 @@ #define TELEMETRY_SRXL #define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK -#define TELEMETRY_IBUS #define USE_RX_MSP #define USE_SERIALRX_JETIEXBUS #define VTX_COMMON diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index b18c29cb6..8e6341e90 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -167,7 +167,7 @@ PG_RESET_TEMPLATE(ibusTelemetryConfig_t, ibusTelemetryConfig, ); */ -#define IBUS_TASK_PERIOD_US (500) +#define IBUS_TASK_PERIOD_US (1000) #define IBUS_UART_MODE (MODE_RXTX) #define IBUS_BAUDRATE (115200) @@ -423,4 +423,4 @@ void freeIbusTelemetryPort(void) ibusTelemetryEnabled = false; } -#endif \ No newline at end of file +#endif diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index fd475d50a..928acdc2b 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -137,6 +137,9 @@ void telemetryCheckState(void) #ifdef TELEMETRY_SRXL checkSrxlTelemetryState(); #endif +#ifdef TELEMETRY_IBUS + checkIbusTelemetryState(); +#endif } void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -170,6 +173,9 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb #ifdef TELEMETRY_SRXL handleSrxlTelemetry(currentTime); #endif +#ifdef TELEMETRY_IBUS + handleIbusTelemetry(); +#endif } #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT) From 3168f6ba88965344855b316da55a471adf562242 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 27 Jan 2017 10:34:08 +1300 Subject: [PATCH 03/55] Fixed 'gyro_sync_denom' limiting when using 32kHz gyro. --- src/main/fc/config.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a4cebaef3..6ee2698c1 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -1078,9 +1078,13 @@ void validateAndFixGyroConfig(void) samplingTime = 0.00003125; // F1 and F3 can't handle high sample speed. #if defined(STM32F1) - gyroConfig()->gyro_sync_denom = constrain(gyroConfig()->gyro_sync_denom, 16, 16); + gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); #elif defined(STM32F3) - gyroConfig()->gyro_sync_denom = constrain(gyroConfig()->gyro_sync_denom, 4, 16); + gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); +#endif + } else { +#if defined(STM32F1) + gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); #endif } From 82df9f0c84a1642e5e1f64da0f6189a26de27184 Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Fri, 27 Jan 2017 10:14:04 +0000 Subject: [PATCH 04/55] Hide BFOSD when OSDSW is active --- src/main/io/osd.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d99a113f4..107f4b7f3 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -377,6 +377,10 @@ void osdDrawElements(void) { displayClearScreen(osdDisplayPort); + /* Hide OSD when OSDSW mode is active */ + if (IS_RC_MODE_ACTIVE(BOXOSD)) + return; + #if 0 if (currentElement) osdDrawElementPositioningHelp(); From 1ad3d74f9f455d1c51d1996eb0e0d8d445167dcd Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Fri, 27 Jan 2017 11:23:28 +0000 Subject: [PATCH 05/55] Add PID and rate profile and battery warn to OSD --- src/main/fc/cli.c | 2 ++ src/main/io/osd.c | 28 ++++++++++++++++++++++++++-- src/main/io/osd.h | 2 ++ 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 2d2dddde4..6788a3cf4 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -793,6 +793,8 @@ const clivalue_t valueTable[] = { { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PIDRATE_PROFILE], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_WARNING], .config.minmax = { 0, OSD_POS_MAX } }, #endif #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } }, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 107f4b7f3..4e94c8436 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -366,6 +366,23 @@ static void osdDrawSingleElement(uint8_t item) break; } + case OSD_PIDRATE_PROFILE: + { + uint8_t profileIndex = masterConfig.current_profile_index; + uint8_t rateProfileIndex = masterConfig.profile[profileIndex].activeRateProfile; + sprintf(buff, "%d-%d", profileIndex + 1, rateProfileIndex + 1); + break; + } + + case OSD_MAIN_BATT_WARNING: + { + if (getVbat() > (batteryWarningVoltage - 1)) + return; + + sprintf(buff, "LOW VOLTAGE"); + break; + } + default: return; } @@ -413,6 +430,8 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_PITCH_PIDS); osdDrawSingleElement(OSD_YAW_PIDS); osdDrawSingleElement(OSD_POWER); + osdDrawSingleElement(OSD_PIDRATE_PROFILE); + osdDrawSingleElement(OSD_MAIN_BATT_WARNING); #ifdef GPS #ifdef CMS @@ -448,6 +467,8 @@ void osdResetConfig(osd_profile_t *osdProfile) osdProfile->item_pos[OSD_PITCH_PIDS] = OSD_POS(2, 11); osdProfile->item_pos[OSD_YAW_PIDS] = OSD_POS(2, 12); osdProfile->item_pos[OSD_POWER] = OSD_POS(15, 1); + osdProfile->item_pos[OSD_PIDRATE_PROFILE] = OSD_POS(2, 13); + osdProfile->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(8, 6); osdProfile->rssi_alarm = 20; osdProfile->cap_alarm = 2200; @@ -506,10 +527,13 @@ void osdUpdateAlarms(void) else pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - if (getVbat() <= (batteryWarningVoltage - 1)) + if (getVbat() <= (batteryWarningVoltage - 1)) { pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; - else + pOsdProfile->item_pos[OSD_MAIN_BATT_WARNING] |= BLINK_FLAG; + } else { pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + pOsdProfile->item_pos[OSD_MAIN_BATT_WARNING] &= ~BLINK_FLAG; + } if (STATE(GPS_FIX) == 0) pOsdProfile->item_pos[OSD_GPS_SATS] |= BLINK_FLAG; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0755ecbe3..756aa0d1c 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -47,6 +47,8 @@ typedef enum { OSD_PITCH_PIDS, OSD_YAW_PIDS, OSD_POWER, + OSD_PIDRATE_PROFILE, + OSD_MAIN_BATT_WARNING, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From ab8f045c4f983a97c123f1d2d4099bd42a4af11c Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 29 Jan 2017 09:33:39 +1100 Subject: [PATCH 06/55] Fixes for REVONANO (thanks @velez for hardware) --- src/main/target/REVONANO/target.h | 30 ++++++++++-------------- src/main/target/REVONANO/target.mk | 3 ++- src/main/target/link/stm32_flash_f411.ld | 3 +-- 3 files changed, 15 insertions(+), 21 deletions(-) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index cfed437f8..f4780ed1a 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -32,33 +32,29 @@ #define INVERTER_PIN_USART2 PC15 //Sbus on USART 2 of nano. -#define MPU9250_CS_PIN PB12 -#define MPU9250_SPI_INSTANCE SPI2 +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_INSTANCE SPI2 #define ACC -#define USE_ACC_MPU9250 -#define USE_ACC_SPI_MPU9250 -#define ACC_MPU9250_ALIGN CW270_DEG +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG #define GYRO -#define USE_GYRO_MPU9250 -#define USE_GYRO_SPI_MPU9250 -#define GYRO_MPU9250_ALIGN CW270_DEG - -//#define MAG -//#define USE_MAG_HMC5883 +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 -// MPU9250 interrupts +// MPU6500 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 #define USE_MPU_DATA_READY_SIGNAL -//#define ENSURE_MPU_DATA_READY_IS_LOW #define USE_VCP -#define VBUS_SENSING_PIN PA9 +//#define VBUS_SENSING_PIN PA9 #define USE_UART1 #define UART1_RX_PIN PB7 @@ -68,15 +64,13 @@ #define UART2_RX_PIN PA3 #define UART2_TX_PIN PA2 -#define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 +#define SERIAL_PORT_COUNT 3 #define USE_ESCSERIAL -#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define ESCSERIAL_TIMER_TX_HARDWARE 0 #define USE_SPI -//#define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 -//#define USE_SPI_DEVICE_3 #define USE_I2C #define I2C_DEVICE (I2CDEV_3) diff --git a/src/main/target/REVONANO/target.mk b/src/main/target/REVONANO/target.mk index 7db95a0ae..e65a2b857 100644 --- a/src/main/target/REVONANO/target.mk +++ b/src/main/target/REVONANO/target.mk @@ -2,5 +2,6 @@ F411_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro_spi_mpu9250.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ drivers/barometer_ms5611.c \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld index 7902c93af..e3744dc21 100644 --- a/src/main/target/link/stm32_flash_f411.ld +++ b/src/main/target/link/stm32_flash_f411.ld @@ -25,10 +25,9 @@ MEMORY FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("STACKRAM", RAM) INCLUDE "stm32_flash.ld" From 1330c8998e887e2daa9cc024f33c25559af7a567 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 29 Jan 2017 09:36:06 +1100 Subject: [PATCH 07/55] Newline --- src/main/target/REVONANO/target.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/REVONANO/target.mk b/src/main/target/REVONANO/target.mk index e65a2b857..47f420b13 100644 --- a/src/main/target/REVONANO/target.mk +++ b/src/main/target/REVONANO/target.mk @@ -4,4 +4,4 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_ms5611.c \ No newline at end of file + drivers/barometer_ms5611.c From 8fb5ea5156fc36db0fd30825b65b2fa5ffc01a8d Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Fri, 27 Jan 2017 16:24:31 +0000 Subject: [PATCH 08/55] Add blackbox storage and device selection to CMS --- src/main/blackbox/blackbox.c | 2 +- src/main/blackbox/blackbox.h | 1 + src/main/cms/cms_menu_blackbox.c | 113 +++++++++++++++++++++++++++++-- 3 files changed, 109 insertions(+), 7 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 3d75bde6b..5b846475c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -755,7 +755,7 @@ static int gcd(int num, int denom) return gcd(denom, num % denom); } -static void validateBlackboxConfig() +void validateBlackboxConfig() { int div; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 1a9690095..388ca2f75 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -32,6 +32,7 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void initBlackbox(void); void handleBlackbox(timeUs_t currentTimeUs); +void validateBlackboxConfig(); void startBlackbox(void); void finishBlackbox(void); diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 230220749..128af8cc2 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -42,8 +43,11 @@ #include "config/config_master.h" #include "config/feature.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "blackbox/blackbox_io.h" + #ifdef USE_FLASHFS static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) { @@ -65,11 +69,92 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) } #endif // USE_FLASHFS +static const char * const cmsx_BlackboxDeviceNames[] = { + "SERIAL", + "FLASH ", + "SDCARD" +}; + static bool featureRead = false; + static uint8_t cmsx_FeatureBlackbox; -static long cmsx_Blackbox_FeatureRead(void) +static uint8_t cmsx_BlackboxDevice; +static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames }; + +#define CMS_BLACKBOX_STATUS_LENGTH 8 +static char cmsx_BlackboxStatus[CMS_BLACKBOX_STATUS_LENGTH]; + +static uint16_t cmsx_BlackboxDeviceStorageTotal; +static uint16_t cmsx_BlackboxDeviceStorageUsed; +static uint16_t cmsx_BlackboxDeviceStorageFree; + +static void cmsx_Blackbox_GetDeviceStatus() { + /* Reset storage counters */ + cmsx_BlackboxDeviceStorageTotal = 0; + cmsx_BlackboxDeviceStorageFree = 0; + cmsx_BlackboxDeviceStorageUsed = 0; + + switch (blackboxConfig()->device) + { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + if (!sdcard_isInserted()) { + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "NO CARD"); + } else if (!sdcard_isFunctional()) { + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "FAULT"); + } else { + switch (afatfs_getFilesystemState()) { + case AFATFS_FILESYSTEM_STATE_READY: + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "READY"); + break; + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "INIT"); + break; + case AFATFS_FILESYSTEM_STATE_FATAL: + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + default: + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "FAULT"); + break; + } + } + + /* Size in MB for SD card */ + cmsx_BlackboxDeviceStorageTotal = sdcard_getMetadata()->numBlocks / 2000; + cmsx_BlackboxDeviceStorageFree = afatfs_getContiguousFreeSpace() / 1024000; + cmsx_BlackboxDeviceStorageUsed = cmsx_BlackboxDeviceStorageTotal - cmsx_BlackboxDeviceStorageFree; + + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + const flashGeometry_t *geometry = flashfsGetGeometry(); + if (flashfsIsReady()) { + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "READY"); + + /* Size in KB for flash */ + cmsx_BlackboxDeviceStorageTotal = geometry->totalSize; + cmsx_BlackboxDeviceStorageUsed = flashfsGetOffset(); + cmsx_BlackboxDeviceStorageFree = cmsx_BlackboxDeviceStorageTotal - cmsx_BlackboxDeviceStorageUsed; + } else { + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "FAULT"); + } + + break; +#endif + + default: + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "---"); + } +} + +static long cmsx_Blackbox_onEnter(void) +{ + cmsx_Blackbox_GetDeviceStatus(); + cmsx_BlackboxDevice = blackboxConfig()->device; + if (!featureRead) { cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0; featureRead = true; @@ -78,6 +163,16 @@ static long cmsx_Blackbox_FeatureRead(void) return 0; } +static long cmsx_Blackbox_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + blackboxConfig()->device = cmsx_BlackboxDevice; + validateBlackboxConfig(); + + return 0; +} + static long cmsx_Blackbox_FeatureWriteback(void) { if (featureRead) { @@ -93,11 +188,16 @@ static long cmsx_Blackbox_FeatureWriteback(void) static OSD_Entry cmsx_menuBlackboxEntries[] = { { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, - { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, - { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom,1,32,1 }, 0 }, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, + { "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, 0 }, + { "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 }, + { "(USED)", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_BlackboxDeviceStorageUsed, 0, 0, 0 }, DYNAMIC }, + { "(FREE)", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_BlackboxDeviceStorageFree, 0, 0, 0 }, DYNAMIC }, + /* { "(TOTAL)", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_BlackboxDeviceStorageTotal, 0, 0, 0 }, DYNAMIC }, */ + { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom, 1, 32, 1 }, 0 }, #ifdef USE_FLASHFS - { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, + { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, #endif // USE_FLASHFS { "BACK", OME_Back, NULL, NULL, 0 }, @@ -107,9 +207,10 @@ static OSD_Entry cmsx_menuBlackboxEntries[] = CMS_Menu cmsx_menuBlackbox = { .GUARD_text = "MENUBB", .GUARD_type = OME_MENU, - .onEnter = cmsx_Blackbox_FeatureRead, - .onExit = NULL, + .onEnter = cmsx_Blackbox_onEnter, + .onExit = cmsx_Blackbox_onExit, .onGlobalExit = cmsx_Blackbox_FeatureWriteback, .entries = cmsx_menuBlackboxEntries }; + #endif From 7f8a6c24fa8012c03578a1dc2a261b10a36136de Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Sat, 28 Jan 2017 09:45:34 +0000 Subject: [PATCH 09/55] Show storage unit, protect blackbox config changes --- src/main/cms/cms_menu_blackbox.c | 85 ++++++++++++++++++-------------- 1 file changed, 47 insertions(+), 38 deletions(-) diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 128af8cc2..9657ec00b 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -82,72 +82,80 @@ static uint8_t cmsx_FeatureBlackbox; static uint8_t cmsx_BlackboxDevice; static OSD_TAB_t cmsx_BlackboxDeviceTable = { &cmsx_BlackboxDevice, 2, cmsx_BlackboxDeviceNames }; -#define CMS_BLACKBOX_STATUS_LENGTH 8 -static char cmsx_BlackboxStatus[CMS_BLACKBOX_STATUS_LENGTH]; - -static uint16_t cmsx_BlackboxDeviceStorageTotal; -static uint16_t cmsx_BlackboxDeviceStorageUsed; -static uint16_t cmsx_BlackboxDeviceStorageFree; +#define CMS_BLACKBOX_STRING_LENGTH 8 +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() { - /* Reset storage counters */ - cmsx_BlackboxDeviceStorageTotal = 0; - cmsx_BlackboxDeviceStorageFree = 0; - cmsx_BlackboxDeviceStorageUsed = 0; + char * unit = "B"; + bool storageDeviceIsWorking = false; + uint16_t storageUsed = 0; + uint16_t storageFree = 0; switch (blackboxConfig()->device) { #ifdef USE_SDCARD case BLACKBOX_DEVICE_SDCARD: + unit = "MB"; + if (!sdcard_isInserted()) { - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "NO CARD"); + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "NO CARD"); } else if (!sdcard_isFunctional()) { - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "FAULT"); + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT"); } else { switch (afatfs_getFilesystemState()) { case AFATFS_FILESYSTEM_STATE_READY: - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "READY"); + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "READY"); + storageDeviceIsWorking = true; break; case AFATFS_FILESYSTEM_STATE_INITIALIZATION: - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "INIT"); + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "INIT"); break; case AFATFS_FILESYSTEM_STATE_FATAL: case AFATFS_FILESYSTEM_STATE_UNKNOWN: default: - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "FAULT"); + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT"); break; } } - /* Size in MB for SD card */ - cmsx_BlackboxDeviceStorageTotal = sdcard_getMetadata()->numBlocks / 2000; - cmsx_BlackboxDeviceStorageFree = afatfs_getContiguousFreeSpace() / 1024000; - cmsx_BlackboxDeviceStorageUsed = cmsx_BlackboxDeviceStorageTotal - cmsx_BlackboxDeviceStorageFree; + if (storageDeviceIsWorking) { + storageFree = afatfs_getContiguousFreeSpace() / 1024000; + storageUsed = (sdcard_getMetadata()->numBlocks / 2000) - storageFree; + } break; #endif #ifdef USE_FLASHFS case BLACKBOX_DEVICE_FLASH: - const flashGeometry_t *geometry = flashfsGetGeometry(); - if (flashfsIsReady()) { - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "READY"); + unit = "KB"; - /* Size in KB for flash */ - cmsx_BlackboxDeviceStorageTotal = geometry->totalSize; - cmsx_BlackboxDeviceStorageUsed = flashfsGetOffset(); - cmsx_BlackboxDeviceStorageFree = cmsx_BlackboxDeviceStorageTotal - cmsx_BlackboxDeviceStorageUsed; + const flashGeometry_t *geometry = flashfsGetGeometry(); + storageDeviceIsWorking = flashfsIsReady(); + + if (storageDeviceIsWorking) { + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "READY"); + + storageUsed = flashfsGetOffset(); + storageFree = geometry->totalSize - storageUsed; } else { - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "FAULT"); + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT"); } break; #endif default: - snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STATUS_LENGTH, "---"); + storageDeviceIsWorking = true; + snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "---"); } + + /* Storage counters */ + snprintf(cmsx_BlackboxDeviceStorageUsed, CMS_BLACKBOX_STRING_LENGTH, "%d%s", storageUsed, unit); + snprintf(cmsx_BlackboxDeviceStorageFree, CMS_BLACKBOX_STRING_LENGTH, "%d%s", storageFree, unit); } static long cmsx_Blackbox_onEnter(void) @@ -167,8 +175,10 @@ static long cmsx_Blackbox_onExit(const OSD_Entry *self) { UNUSED(self); - blackboxConfig()->device = cmsx_BlackboxDevice; - validateBlackboxConfig(); + if (blackboxMayEditConfig()) { + blackboxConfig()->device = cmsx_BlackboxDevice; + validateBlackboxConfig(); + } return 0; } @@ -188,16 +198,15 @@ static long cmsx_Blackbox_FeatureWriteback(void) static OSD_Entry cmsx_menuBlackboxEntries[] = { { "-- BLACKBOX --", OME_Label, NULL, NULL, 0}, - { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, - { "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, 0 }, - { "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 }, - { "(USED)", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_BlackboxDeviceStorageUsed, 0, 0, 0 }, DYNAMIC }, - { "(FREE)", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_BlackboxDeviceStorageFree, 0, 0, 0 }, DYNAMIC }, - /* { "(TOTAL)", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_BlackboxDeviceStorageTotal, 0, 0, 0 }, DYNAMIC }, */ - { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom, 1, 32, 1 }, 0 }, + { "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 }, + { "DEVICE", OME_TAB, NULL, &cmsx_BlackboxDeviceTable, 0 }, + { "(STATUS)", OME_String, NULL, &cmsx_BlackboxStatus, 0 }, + { "(USED)", OME_String, NULL, &cmsx_BlackboxDeviceStorageUsed, 0 }, + { "(FREE)", OME_String, NULL, &cmsx_BlackboxDeviceStorageFree, 0 }, + { "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom, 1, 32, 1 }, 0 }, #ifdef USE_FLASHFS - { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, + { "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 }, #endif // USE_FLASHFS { "BACK", OME_Back, NULL, NULL, 0 }, From 7e69f3bb6e5a054e9a2d8ac116a2690d776d2182 Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Sat, 28 Jan 2017 10:48:39 +0000 Subject: [PATCH 10/55] Correct filesize conversion for flash --- src/main/cms/cms_menu_blackbox.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 9657ec00b..d1ea564a8 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -91,8 +91,8 @@ static void cmsx_Blackbox_GetDeviceStatus() { char * unit = "B"; bool storageDeviceIsWorking = false; - uint16_t storageUsed = 0; - uint16_t storageFree = 0; + uint32_t storageUsed = 0; + uint32_t storageFree = 0; switch (blackboxConfig()->device) { @@ -133,14 +133,13 @@ static void cmsx_Blackbox_GetDeviceStatus() case BLACKBOX_DEVICE_FLASH: unit = "KB"; - const flashGeometry_t *geometry = flashfsGetGeometry(); storageDeviceIsWorking = flashfsIsReady(); - if (storageDeviceIsWorking) { snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "READY"); - storageUsed = flashfsGetOffset(); - storageFree = geometry->totalSize - storageUsed; + const flashGeometry_t *geometry = flashfsGetGeometry(); + storageUsed = flashfsGetOffset() / 1024; + storageFree = (geometry->totalSize / 1024) - storageUsed; } else { snprintf(cmsx_BlackboxStatus, CMS_BLACKBOX_STRING_LENGTH, "FAULT"); } @@ -154,8 +153,8 @@ static void cmsx_Blackbox_GetDeviceStatus() } /* Storage counters */ - snprintf(cmsx_BlackboxDeviceStorageUsed, CMS_BLACKBOX_STRING_LENGTH, "%d%s", storageUsed, unit); - snprintf(cmsx_BlackboxDeviceStorageFree, CMS_BLACKBOX_STRING_LENGTH, "%d%s", storageFree, unit); + snprintf(cmsx_BlackboxDeviceStorageUsed, CMS_BLACKBOX_STRING_LENGTH, "%ld%s", storageUsed, unit); + snprintf(cmsx_BlackboxDeviceStorageFree, CMS_BLACKBOX_STRING_LENGTH, "%ld%s", storageFree, unit); } static long cmsx_Blackbox_onEnter(void) From 6295b7b98f093ab9100b342eb4efa66596e8fbea Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 29 Jan 2017 00:50:30 +0100 Subject: [PATCH 11/55] Set version // undefine GPS on BFF3 // Bump EEPROM --- src/main/build/version.h | 2 +- src/main/config/config_eeprom.h | 2 +- src/main/target/BETAFLIGHTF3/target.h | 2 ++ src/main/target/common.h | 1 + 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index 59a418b26..3df77b8cf 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -18,7 +18,7 @@ #define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 8d73a3196..645689ffd 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,7 +17,7 @@ #pragma once -#define EEPROM_CONF_VERSION 153 +#define EEPROM_CONF_VERSION 154 void initEEPROM(void); void writeEEPROM(); diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index a81ea68ea..381edc8eb 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -130,6 +130,8 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#undef GPS + // IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/common.h b/src/main/target/common.h index 1a1a9ac57..a51e21ae6 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -49,6 +49,7 @@ #ifdef STM32F3 #define USE_DSHOT +#undef GPS #endif #ifdef STM32F1 From b24cdcdbd354bf50f076461093865e2f7fbf3b09 Mon Sep 17 00:00:00 2001 From: fishpepper Date: Fri, 27 Jan 2017 15:40:13 +0100 Subject: [PATCH 12/55] fixes led strip --- src/main/target/TINYFISH/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/TINYFISH/target.c b/src/main/target/TINYFISH/target.c index 8f16d1053..15a83024b 100644 --- a/src/main/target/TINYFISH/target.c +++ b/src/main/target/TINYFISH/target.c @@ -30,6 +30,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH7 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), //DMA1_CH1 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, TIMER_OUTPUT_ENABLED) //DMA1_CH2 - LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, TIMER_OUTPUT_ENABLED) //DMA1_CH2 - LED }; From 990c13b7ead0c34526b29d5cf8f70a730d0a3a2a Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 22 Jan 2017 17:29:39 +1300 Subject: [PATCH 13/55] Set MINIMAL_CLI to be default for F3. Added more size reductions to MINIMAL_CLI. --- src/main/fc/cli.c | 70 +++++++++++++-------------- src/main/target/BETAFLIGHTF3/target.h | 2 - src/main/target/common.h | 5 +- 3 files changed, 36 insertions(+), 41 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6788a3cf4..6b6cc8773 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -821,7 +821,7 @@ static void cliPrint(const char *str) bufWriterFlush(cliWriter); } -#ifdef CLI_MINIMAL_VERBOSITY +#ifdef MINIMAL_CLI #define cliPrintHashLine(str) #else static void cliPrintHashLine(const char *str) @@ -1082,7 +1082,7 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value) } } -#ifndef CLI_MINIMAL_VERBOSITY +#ifndef MINIMAL_CLI static void cliRepeat(char ch, uint8_t len) { for (int i = 0; i < len; i++) { @@ -1104,7 +1104,7 @@ static void cliShowParseError(void) static void cliShowArgumentRangeError(char *name, int min, int max) { - cliPrintf("%s must be between %d and %d\r\n", name, min, max); + cliPrintf("%s not between %d and %d\r\n", name, min, max); } static char *nextArg(char *currentArg) @@ -1497,7 +1497,7 @@ static void cliSerialPassthrough(char *cmdline) serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id); if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) { if (!baud) { - printf("Port %d is not open, you must specify baud\r\n", id); + printf("Port %d is closed, must specify baud.\r\n", id); return; } if (!mode) @@ -1507,30 +1507,29 @@ static void cliSerialPassthrough(char *cmdline) baud, mode, SERIAL_NOT_INVERTED); if (!passThroughPort) { - printf("Port %d could not be opened\r\n", id); + printf("Port %d could not be opened.\r\n", id); return; } - printf("Port %d opened, baud=%d\r\n", id, baud); + printf("Port %d opened, baud = %d.\r\n", id, baud); } else { passThroughPort = passThroughPortUsage->serialPort; // If the user supplied a mode, override the port's mode, otherwise // leave the mode unchanged. serialPassthrough() handles one-way ports. - printf("Port %d already open\r\n", id); + printf("Port %d already open.\r\n", id); if (mode && passThroughPort->mode != mode) { - printf("Adjusting mode from configured value %d to %d\r\n", + printf("Adjusting mode from %d to %d.\r\n", passThroughPort->mode, mode); serialSetMode(passThroughPort, mode); } // If this port has a rx callback associated we need to remove it now. // Otherwise no data will be pushed in the serial port buffer! if (passThroughPort->rxCallback) { - printf("Removing rxCallback from port\r\n"); + printf("Removing rxCallback\r\n"); passThroughPort->rxCallback = 0; } } - printf("Relaying data to device on port %d, Reset your board to exit " - "serial passthrough mode.\r\n", id); + printf("Forwarding data to %d, power cycle to exit.\r\n", id); serialPassthrough(cliPort, passThroughPort, NULL, NULL); } @@ -2343,7 +2342,7 @@ static void cliFlashErase(char *cmdline) { UNUSED(cmdline); -#ifndef CLI_MINIMAL_VERBOSITY +#ifndef MINIMAL_CLI uint32_t i; cliPrintf("Erasing, please wait ... \r\n"); #else @@ -2354,7 +2353,7 @@ static void cliFlashErase(char *cmdline) flashfsEraseCompletely(); while (!flashfsIsReady()) { -#ifndef CLI_MINIMAL_VERBOSITY +#ifndef MINIMAL_CLI cliPrintf("."); if (i++ > 120) { i=0; @@ -2855,14 +2854,14 @@ static void cliEscPassthrough(char *cmdline) index = atoi(pch); if(mode == 2 && index == 255) { - printf("passthru on all pwm outputs enabled\r\n"); + printf("passthrough on all outputs enabled\r\n"); } else{ if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { - printf("passthru at pwm output %d enabled\r\n", index); + printf("passthrough on output %d enabled\r\n", index); } else { - printf("invalid pwm output, valid range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); + printf("invalid output, range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); return; } } @@ -2955,7 +2954,7 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); } -#if (FLASH_SIZE > 128) +#ifndef MINIMAL_CLI static void cliPlaySound(char *cmdline) { int i; @@ -3247,11 +3246,11 @@ static void cliTasks(char *cmdline) int maxLoadSum = 0; int averageLoadSum = 0; -#ifndef CLI_MINIMAL_VERBOSITY +#ifndef MINIMAL_CLI if (masterConfig.task_statistics) { cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); } else { - cliPrintf("Task list rate/hz\r\n"); + cliPrintf("Task list\r\n"); } #endif for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { @@ -3393,7 +3392,6 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig) } } -#ifndef CLI_MINIMAL_VERBOSITY static bool resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { const char * format = "\r\n* %s * %c%d also used by %s"; @@ -3421,7 +3419,6 @@ static bool resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) } return true; } -#endif static void cliResource(char *cmdline) { @@ -3432,7 +3429,9 @@ static void cliResource(char *cmdline) return; } else if (strncasecmp(cmdline, "list", len) == 0) { -#ifndef CLI_MINIMAL_VERBOSITY +#ifdef MINIMAL_CLI + cliPrintf("Resources:\r\n"); +#else cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n"); cliRepeat('-', 20); #endif @@ -3447,8 +3446,10 @@ static void cliResource(char *cmdline) } } +#ifdef MINIMAL_CLI + cliPrintf("\r\n\r\nDMA:\r\n"); +#else cliPrintf("\r\n\r\nCurrently active DMA:\r\n"); -#ifndef CLI_MINIMAL_VERBOSITY cliRepeat('-', 20); #endif for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { @@ -3464,7 +3465,7 @@ static void cliResource(char *cmdline) } } -#ifndef CLI_MINIMAL_VERBOSITY +#ifndef MINIMAL_CLI cliPrintf("\r\nUse: 'resource' to see how to change resources.\r\n"); #endif @@ -3506,7 +3507,7 @@ static void cliResource(char *cmdline) if (strlen(pch) > 0) { if (strcasecmp(pch, "NONE") == 0) { *tag = IO_TAG_NONE; - cliPrintf("Resource is freed!\r\n"); + cliPrintf("Resource freed.\r\n"); return; } else { uint8_t port = (*pch) - 'A'; @@ -3520,17 +3521,14 @@ static void cliResource(char *cmdline) if (pin < 16) { ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin))); if (rec) { -#ifndef CLI_MINIMAL_VERBOSITY if (!resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin))) { return; } - cliPrintf("\r\nResource is set to %c%02d!\r\n", port + 'A', pin); -#else - cliPrintf("Set to %c%02d!", port + 'A', pin); -#endif + cliPrintf("\r\nResource set to %c%02d!\r\n", port + 'A', pin); + *tag = DEFIO_TAG_MAKE(port, pin); } else { - cliPrintf("Resource is invalid!\r\n"); + cliPrintf("Resource invalid!\r\n"); } return; } @@ -3701,14 +3699,14 @@ static void cliDiff(char *cmdline) typedef struct { const char *name; -#ifndef SKIP_CLI_COMMAND_HELP +#ifndef MINIMAL_CLI const char *description; const char *args; #endif void (*func)(char *cmdline); } clicmd_t; -#ifndef SKIP_CLI_COMMAND_HELP +#ifndef MINIMAL_CLI #define CLI_COMMAND_DEF(name, description, args, method) \ { \ name , \ @@ -3776,7 +3774,7 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), CLI_COMMAND_DEF("name", "name of craft", NULL, cliName), -#if (FLASH_SIZE > 128) +#ifndef MINIMAL_CLI CLI_COMMAND_DEF("play_sound", NULL, "[]", cliPlaySound), #endif CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), @@ -3821,7 +3819,7 @@ static void cliHelp(char *cmdline) for (uint32_t i = 0; i < CMD_COUNT; i++) { cliPrint(cmdTable[i].name); -#ifndef SKIP_CLI_COMMAND_HELP +#ifndef MINIMAL_CLI if (cmdTable[i].description) { cliPrintf(" - %s", cmdTable[i].description); } @@ -3952,7 +3950,7 @@ void cliEnter(serialPort_t *serialPort) schedulerSetCalulateTaskStatistics(masterConfig.task_statistics); -#ifndef CLI_MINIMAL_VERBOSITY +#ifndef MINIMAL_CLI cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n"); #else cliPrint("\r\nCLI\r\n"); diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 381edc8eb..a81ea68ea 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -130,8 +130,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#undef GPS - // IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/common.h b/src/main/target/common.h index a51e21ae6..a9270aa8a 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -50,6 +50,7 @@ #ifdef STM32F3 #define USE_DSHOT #undef GPS +#define MINIMAL_CLI #endif #ifdef STM32F1 @@ -57,7 +58,7 @@ #define USE_UART1_RX_DMA #define USE_UART1_TX_DMA -#define CLI_MINIMAL_VERBOSITY +#define MINIMAL_CLI #endif #define SERIAL_RX @@ -109,7 +110,5 @@ #define VTX_SMARTAUDIO #define VTX_TRAMP #define USE_SENSOR_NAMES -#else -#define SKIP_CLI_COMMAND_HELP #endif From 9dfb3e45ee08e9cc75ce0aede4126d3ad33ac9c3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 26 Jan 2017 18:15:29 +0000 Subject: [PATCH 14/55] Add ITerm anti-windup based on motor output saturation Added noise threshold for PID ITerm accumulation Removed ITerm setpoint scaler. Added CLI and MSP settings Made default ITerm noise threshold zero. Added CLI setting. Removed itermWindupPointPercent from MSP --- src/main/blackbox/blackbox.c | 3 +-- src/main/fc/cli.c | 4 ++-- src/main/fc/config.c | 4 ++-- src/main/fc/fc_msp.c | 8 ++++---- src/main/flight/mixer.c | 8 +++++++- src/main/flight/mixer.h | 2 ++ src/main/flight/pid.c | 28 +++++++++++++++++++--------- src/main/flight/pid.h | 4 ++-- 8 files changed, 39 insertions(+), 22 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5b846475c..49ceee084 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1250,8 +1250,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", currentProfile->pidProfile.rollPitchItermIgnoreRate); - BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", currentProfile->pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentProfile->pidProfile.itermWindupPointPercent); BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count); BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6b6cc8773..17cdfd794 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -707,8 +707,8 @@ const clivalue_t valueTable[] = { { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, - { "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, - { "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {50, 100 } }, + { "iterm_noise_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermNoiseThreshold, .config.minmax = {0, 20 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 6ee2698c1..4ce27a28b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -169,8 +169,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->yaw_lpf_hz = 0; - pidProfile->rollPitchItermIgnoreRate = 200; - pidProfile->yawItermIgnoreRate = 55; + pidProfile->itermWindupPointPercent = 75; + pidProfile->itermNoiseThreshold = 0; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 260; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c4a4599e8..332f24eb8 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1159,8 +1159,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_PID_ADVANCED: - sbufWriteU16(dst, currentProfile->pidProfile.rollPitchItermIgnoreRate); - sbufWriteU16(dst, currentProfile->pidProfile.yawItermIgnoreRate); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit); sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation); @@ -1515,8 +1515,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_PID_ADVANCED: - currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src); - currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src); sbufReadU8(src); // reserved currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8ee4bb574..accc55505 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -56,6 +56,7 @@ #define EXTERNAL_CONVERSION_MAX_VALUE 2000 static uint8_t motorCount; +static float motorMixRange; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; @@ -245,6 +246,11 @@ uint8_t getMotorCount() return motorCount; } +float getMotorMixRange() +{ + return motorMixRange; +} + bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT switch(motorConfig->motorPwmProtocol) { @@ -500,7 +506,7 @@ void mixTable(pidProfile_t *pidProfile) } } - const float motorMixRange = motorMixMax - motorMixMin; + motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index da4c04c15..8a84ea8ac 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -113,10 +113,12 @@ typedef struct airplaneConfig_s { extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; + struct motorConfig_s; struct rxConfig_s; uint8_t getMotorCount(); +float getMotorMixRange(); void mixerUseConfigs( flight3DConfig_t *flight3DConfigToUse, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e3415cb01..3ae40d0a0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,6 +34,7 @@ #include "flight/pid.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/navigation.h" #include "sensors/gyro.h" @@ -146,7 +147,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } -static float Kp[3], Ki[3], Kd[3], c[3], levelGain, horizonGain, horizonTransition, maxVelocity[3], relaxFactor[3]; +static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3]; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermNoiseThresholdDps; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -161,6 +163,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) { horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL]; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; + ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; + ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f; } static float calcHorizonLevelStrength(void) { @@ -238,15 +242,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } // -----calculate I component - // Reduce strong Iterm accumulation during higher stick inputs - const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f); - float ITerm = previousGyroIf[axis]; - ITerm += Ki[axis] * errorRate * dT * setpointRateScaler * itermAccelerator; - // limit maximum integrator value to prevent WindUp - ITerm = constrainf(ITerm, -250.0f, 250.0f); - previousGyroIf[axis] = ITerm; + const float motorMixRangeAbs = ABS(getMotorMixRange()); + if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) { + // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold + // Reduce strong Iterm accumulation during higher stick inputs + float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator; + // gradually scale back integration when above windup point (default is 75%) + if (motorMixRangeAbs > ITermWindupPoint) { + ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint); + } + ITerm += ITermDelta; + // also limit maximum integrator value to prevent windup + ITerm = constrainf(ITerm, -250.0f, 250.0f); + previousGyroIf[axis] = ITerm; + } // -----calculate D component (Yaw D not yet supported) float DTerm = 0.0; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 435cc7f9a..87ac84f2e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -66,8 +66,8 @@ typedef struct pidProfile_s { uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff - uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates - uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates + uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation + uint8_t itermNoiseThreshold; // Experimental ITerm noise threshold uint16_t yaw_p_limit; float pidSumLimit; uint8_t dterm_average_count; // Configurable delta count for dterm From ad892400e5820d7ce675eeb2be9a64a2e9b13f4c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 29 Jan 2017 02:15:57 +0100 Subject: [PATCH 15/55] Cleanup and optimize new anti windup // Simplify relaxFactor to Dterm --- src/main/fc/cli.c | 3 +-- src/main/fc/config.c | 3 +-- src/main/flight/pid.c | 19 +++++++++---------- src/main/flight/pid.h | 1 - 4 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 17cdfd794..bddc4c6d5 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -707,8 +707,7 @@ const clivalue_t valueTable[] = { { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, - { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {50, 100 } }, - { "iterm_noise_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermNoiseThreshold, .config.minmax = {0, 20 } }, + { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {30, 100 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 4ce27a28b..1de72a316 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -169,8 +169,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->yaw_lpf_hz = 0; - pidProfile->itermWindupPointPercent = 75; - pidProfile->itermNoiseThreshold = 0; + pidProfile->itermWindupPointPercent = 50; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 260; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3ae40d0a0..ddc2fc0e5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -148,7 +148,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) } static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3]; -static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermNoiseThresholdDps; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -164,7 +164,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) { maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; - ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f; } static float calcHorizonLevelStrength(void) { @@ -215,6 +214,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an static float previousRateError[2]; static float previousSetpoint[3]; + const float motorMixRange = getMotorMixRange(); + // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { float currentPidSetpoint = getSetpointRate(axis); @@ -243,14 +244,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate I component float ITerm = previousGyroIf[axis]; - const float motorMixRangeAbs = ABS(getMotorMixRange()); - if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) { + if (motorMixRange < 1.0f) { // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold - // Reduce strong Iterm accumulation during higher stick inputs float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator; - // gradually scale back integration when above windup point (default is 75%) - if (motorMixRangeAbs > ITermWindupPoint) { - ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint); + // gradually scale back integration when above windup point + if (motorMixRange > ITermWindupPoint) { + ITermDelta *= (1.0f - motorMixRange) * ITermWindupPoint; } ITerm += ITermDelta; // also limit maximum integrator value to prevent windup @@ -267,10 +266,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an dynC = c[axis]; if (currentPidSetpoint > 0) { if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis]) - dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + dynC *= (1.0f - rcDeflection) * relaxFactor[axis]; } else if (currentPidSetpoint < 0) { if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis]) - dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + dynC *= (1.0f - rcDeflection) * relaxFactor[axis]; } } const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 87ac84f2e..4cb7db3a9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -67,7 +67,6 @@ typedef struct pidProfile_s { uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation - uint8_t itermNoiseThreshold; // Experimental ITerm noise threshold uint16_t yaw_p_limit; float pidSumLimit; uint8_t dterm_average_count; // Configurable delta count for dterm From ff8be19b8f2fe7698a4de12d263f0a876efd07dd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 29 Jan 2017 13:45:54 +0100 Subject: [PATCH 16/55] Optimise / fix new transition calculations // Dont use accelerator during anti windup --- src/main/fc/config.c | 2 +- src/main/flight/pid.c | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 1de72a316..d7baf5ec4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -183,7 +183,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawRateAccelLimit = 10.0f; pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 300; - pidProfile->itermAcceleratorGain = 4.0f; + pidProfile->itermAcceleratorGain = 3.0f; } void resetProfile(profile_t *profile) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ddc2fc0e5..930e6991f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -148,7 +148,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) } static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3]; -static float levelGain, horizonGain, horizonTransition, ITermWindupPoint; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -156,7 +156,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); + relaxFactor[axis] = pidProfile->setpointRelaxRatio / 100.0f; } levelGain = pidProfile->P8[PIDLEVEL] / 10.0f; horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f; @@ -164,6 +164,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; + ITermWindupPointInv = 1.0f - ITermWindupPoint; } static float calcHorizonLevelStrength(void) { @@ -246,10 +247,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an float ITerm = previousGyroIf[axis]; if (motorMixRange < 1.0f) { // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold - float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator; + float ITermDelta = Ki[axis] * errorRate * dT; // gradually scale back integration when above windup point if (motorMixRange > ITermWindupPoint) { - ITermDelta *= (1.0f - motorMixRange) * ITermWindupPoint; + ITermDelta *= (1.0f - motorMixRange) / ITermWindupPointInv; + } else { + ITermDelta *= itermAccelerator; } ITerm += ITermDelta; // also limit maximum integrator value to prevent windup @@ -266,10 +269,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an dynC = c[axis]; if (currentPidSetpoint > 0) { if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis]) - dynC *= (1.0f - rcDeflection) * relaxFactor[axis]; + dynC *= MIN((1.0f - rcDeflection) / relaxFactor[axis], 1.0f); } else if (currentPidSetpoint < 0) { if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis]) - dynC *= (1.0f - rcDeflection) * relaxFactor[axis]; + dynC *= MIN((1.0f - rcDeflection) / relaxFactor[axis], 1.0f); } } const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y From 12a6d2f848f48deeb3e8d8682455694f1d7a37e1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 01:09:03 +0100 Subject: [PATCH 17/55] Increase default Idle offset --- src/main/fc/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d7baf5ec4..6e554b570 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -280,7 +280,7 @@ void resetMotorConfig(motorConfig_t *motorConfig) #endif motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; - motorConfig->digitalIdleOffsetPercent = 3.0f; + motorConfig->digitalIdleOffsetPercent = 4.5f; int motorIndex = 0; for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) { From cf04294f70d14ad138de816cec494cb649c1aa2b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 09:21:07 +0100 Subject: [PATCH 18/55] Prevent ItermAccel and windup to clash with each other --- src/main/fc/cli.c | 1 + src/main/fc/config.c | 1 + src/main/flight/pid.c | 16 ++++++++-------- src/main/flight/pid.h | 3 ++- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index bddc4c6d5..bbc71158d 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -702,6 +702,7 @@ const clivalue_t valueTable[] = { { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, + { "anti_gravity_rate_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorRateLimit, .config.minmax = {0, 2000 } }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 6e554b570..e4d2b69be 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -184,6 +184,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 300; pidProfile->itermAcceleratorGain = 3.0f; + pidProfile->itermAcceleratorRateLimit = 80; } void resetProfile(profile_t *profile) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 930e6991f..3ab743ff2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -148,7 +148,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) } static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3]; -static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -165,6 +165,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPointInv = 1.0f - ITermWindupPoint; + itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit; } static float calcHorizonLevelStrength(void) { @@ -216,6 +217,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an static float previousSetpoint[3]; const float motorMixRange = getMotorMixRange(); + // Dynamic ki component to gradually scale back integration when above windup point + float dynKi = MIN((1.0f - motorMixRange) / ITermWindupPointInv, 1.0f); // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -247,13 +250,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an float ITerm = previousGyroIf[axis]; if (motorMixRange < 1.0f) { // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold - float ITermDelta = Ki[axis] * errorRate * dT; - // gradually scale back integration when above windup point - if (motorMixRange > ITermWindupPoint) { - ITermDelta *= (1.0f - motorMixRange) / ITermWindupPointInv; - } else { - ITermDelta *= itermAccelerator; - } + // Iterm will only be accelerated below steady rate threshold + if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) + dynKi *= itermAccelerator; + float ITermDelta = Ki[axis] * errorRate * dT * dynKi; ITerm += ITermDelta; // also limit maximum integrator value to prevent windup ITerm = constrainf(ITerm, -250.0f, 250.0f); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4cb7db3a9..6162d25e3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,8 +76,9 @@ typedef struct pidProfile_s { uint8_t levelSensitivity; // Angle mode sensitivity reflected in degrees assuming user using full stick // Betaflight PID controller parameters - uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms + uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms float itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit + uint16_t itermAcceleratorRateLimit; // Setpointrate limit for iterm accelerator to operate within uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) float yawRateAccelLimit; // yaw accel limiter for deg/sec/ms From d1944b532ca432093e0ed5da72bcfc29bf3a05a3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 09:42:40 +0100 Subject: [PATCH 19/55] Simplify cli names // flash space savings --- src/main/fc/cli.c | 80 +++++++++++++++++++++++------------------------ 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index bbc71158d..022dff6de 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -499,14 +499,14 @@ const clivalue_t valueTable[] = { { "max_check", VAR_UINT16 | MASTER_VALUE, &rxConfig()->maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &rxConfig()->rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, - { "rc_interpolation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } }, - { "rc_interpolation_channels", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolationChannels, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS } }, - { "rc_interpolation_interval", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rcInterpolationInterval, .config.minmax = { 1, 50 } }, + { "rc_interp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } }, + { "rc_interp_ch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rcInterpolationChannels, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS } }, + { "rc_interp_int", VAR_UINT8 | MASTER_VALUE, &rxConfig()->rcInterpolationInterval, .config.minmax = { 1, 50 } }, { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &rxConfig()->rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } }, #if defined(USE_PWM) { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, #endif - { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &rxConfig()->fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, + { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, &rxConfig()->fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &rxConfig()->max_aux_channel, .config.minmax = { 0, 13 } }, { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, @@ -570,41 +570,41 @@ const clivalue_t valueTable[] = { #ifdef SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, - { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind_autoreset, .config.minmax = { 0, 1} }, + { "spektrum_sat_bind_autorst", VAR_UINT8 | MASTER_VALUE, &rxConfig()->spektrum_sat_bind_autoreset, .config.minmax = { 0, 1} }, #endif #ifdef TELEMETRY - { "telemetry_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_switch, .config.lookup = { TABLE_OFF_ON } }, - { "telemetry_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_inversion, .config.lookup = { TABLE_OFF_ON } }, + { "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_switch, .config.lookup = { TABLE_OFF_ON } }, + { "tlm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->telemetry_inversion, .config.lookup = { TABLE_OFF_ON } }, { "sport_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->sportHalfDuplex, .config.lookup = { TABLE_OFF_ON } }, - { "frsky_default_lattitude", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLatitude, .config.minmax = { -90.0, 90.0 } }, - { "frsky_default_longitude", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLongitude, .config.minmax = { -180.0, 180.0 } }, - { "frsky_coordinates_format", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_coordinate_format, .config.minmax = { 0, FRSKY_FORMAT_NMEA } }, + { "frsky_default_lat", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLatitude, .config.minmax = { -90.0, 90.0 } }, + { "frsky_default_long", VAR_FLOAT | MASTER_VALUE, &telemetryConfig()->gpsNoFixLongitude, .config.minmax = { -180.0, 180.0 } }, + { "frsky_gps_format", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_coordinate_format, .config.minmax = { 0, FRSKY_FORMAT_NMEA } }, { "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_unit, .config.lookup = { TABLE_UNIT } }, { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH } }, { "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, - { "hott_alarm_sound_interval", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, - { "pid_values_as_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, + { "hott_alarm_sound_int", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, + { "pid_in_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, #if defined(TELEMETRY_IBUS) { "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, #endif #endif - { "battery_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, + { "vbat_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } }, { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmaxcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbathysteresis, .config.minmax = { 0, 250 } }, - { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } }, - { "current_meter_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, - { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, + { "current_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } }, + { "current_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, + { "mwii_current_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, { "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->batteryMeterType, .config.lookup = { TABLE_BATTERY_SENSOR } }, - { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, + { "no_vbat_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, - { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, - { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, + { "vbat_usage_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "vbat_usage_percent", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, @@ -622,10 +622,10 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 16000 } }, - { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_1, .config.minmax = { 1, 16000 } }, + { "gyro_notch1_cut", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_1, .config.minmax = { 1, 16000 } }, { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_2, .config.minmax = { 0, 16000 } }, - { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_2, .config.minmax = { 1, 16000 } }, - { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, + { "gyro_notch2_cut", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_2, .config.minmax = { 1, 16000 } }, + { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyroMovementCalibrationThreshold, .config.minmax = { 0, 200 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp, .config.minmax = { 0, 32000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki, .config.minmax = { 0, 32000 } }, @@ -634,8 +634,8 @@ const clivalue_t valueTable[] = { { "deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->deadband, .config.minmax = { 0, 32 } }, { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->yaw_deadband, .config.minmax = { 0, 100 } }, - { "throttle_correction_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } }, - { "throttle_correction_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } }, + { "throttle_correct_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } }, + { "throttle_correct_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } }, @@ -645,8 +645,8 @@ const clivalue_t valueTable[] = { #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, - { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} }, - { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, + { "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} }, + { "servo_lowpass", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } }, #endif @@ -662,7 +662,7 @@ const clivalue_t valueTable[] = { { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, - { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &rxConfig()->airModeActivateThreshold, .config.minmax = {1000, 2000 } }, + { "airmode_start_throttle", VAR_UINT16 | MASTER_VALUE, &rxConfig()->airModeActivateThreshold, .config.minmax = {1000, 2000 } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_delay, .config.minmax = { 0, 200 } }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &failsafeConfig()->failsafe_off_delay, .config.minmax = { 0, 200 } }, @@ -694,17 +694,17 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } }, #endif - { "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, - { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 16000 } }, - { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 16000 } }, - { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, - { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, + { "d_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "d_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 16000 } }, + { "d_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 16000 } }, + { "d_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, + { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, - { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, + { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, { "anti_gravity_gain", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } }, { "anti_gravity_rate_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorRateLimit, .config.minmax = {0, 2000 } }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, + { "d_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, @@ -734,8 +734,8 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, - { "level_stick_sensitivity", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10, 200 } }, - { "level_angle_limit", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10, 120 } }, + { "level_sensitivity", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10, 200 } }, + { "level_limit", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10, 120 } }, #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } }, @@ -774,16 +774,16 @@ const clivalue_t valueTable[] = { { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->time_alarm, .config.minmax = { 0, 60 } }, { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->alt_alarm, .config.minmax = { 0, 10000 } }, - { "osd_main_voltage_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_ontime_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_artificial_horizon", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_current_draw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_horizon_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_current_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POS_MAX } }, { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POS_MAX } }, From 638ed22fa886bf29873d7892889c7a0efbdecb93 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 10:50:35 +0100 Subject: [PATCH 20/55] Minor pid init precalculation optimalisations // Simplify dynC --- src/main/flight/pid.c | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3ab743ff2..72a2adf6e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -156,7 +156,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - relaxFactor[axis] = pidProfile->setpointRelaxRatio / 100.0f; + relaxFactor[axis] = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f); } levelGain = pidProfile->P8[PIDLEVEL] / 10.0f; horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f; @@ -164,7 +164,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; - ITermWindupPointInv = 1.0f - ITermWindupPoint; + ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit; } @@ -214,11 +214,10 @@ static float accelerationLimit(int axis, float currentPidSetpoint) { void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor) { static float previousRateError[2]; - static float previousSetpoint[3]; const float motorMixRange = getMotorMixRange(); // Dynamic ki component to gradually scale back integration when above windup point - float dynKi = MIN((1.0f - motorMixRange) / ITermWindupPointInv, 1.0f); + float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -265,15 +264,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an if (axis != FD_YAW) { float dynC = c[axis]; if (pidProfile->setpointRelaxRatio < 100) { - const float rcDeflection = getRcDeflectionAbs(axis); - dynC = c[axis]; - if (currentPidSetpoint > 0) { - if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis]) - dynC *= MIN((1.0f - rcDeflection) / relaxFactor[axis], 1.0f); - } else if (currentPidSetpoint < 0) { - if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis]) - dynC *= MIN((1.0f - rcDeflection) / relaxFactor[axis], 1.0f); - } + dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor[axis], 1.0f); } const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y // Divide rate change by dT to get differential (ie dr/dt) @@ -288,7 +279,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); } - previousSetpoint[axis] = currentPidSetpoint; // -----calculate total PID output axisPIDf[axis] = PTerm + ITerm + DTerm; From a7f50ad30aef7a64fd8045e687b9f197d63765a2 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 30 Jan 2017 10:59:38 +1100 Subject: [PATCH 21/55] Mixer protection for not enough motors initialised --- src/main/drivers/pwm_output.c | 13 ++++++++++++- src/main/drivers/pwm_output_stm32f3xx.c | 2 +- src/main/drivers/pwm_output_stm32f4xx.c | 2 +- src/main/drivers/pwm_output_stm32f7xx.c | 2 +- 4 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2c7628988..befbbedee 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -17,6 +17,7 @@ #include #include +#include #include #include "platform.h" @@ -134,7 +135,9 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - pwmWritePtr(index, value); + if (motors[index].enabled) { + pwmWritePtr(index, value); + } } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) @@ -166,6 +169,11 @@ bool pwmAreMotorsEnabled(void) static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { for (int index = 0; index < motorCount; index++) { + + if (!motors[index].enabled) { + continue; + } + bool overflowed = false; // If we have not already overflowed this timer for (int j = 0; j < index; j++) { @@ -192,6 +200,8 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { + memset(motors, 0, sizeof(motors)); + uint32_t timerMhzCounter = 0; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool isDigital = false; @@ -277,6 +287,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot } motors[motorIndex].enabled = true; } + pwmMotorsEnabled = true; } diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 7a624c363..5ae299dcf 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -61,7 +61,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value) motorDmaOutput_t * const motor = &dmaMotors[index]; - if (!motor->timerHardware->dmaChannel) { + if (!motor->timerHardware || !motor->timerHardware->dmaChannel) { return; } diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 55a48bcba..03156953a 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -59,7 +59,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value) motorDmaOutput_t * const motor = &dmaMotors[index]; - if (!motor->timerHardware->dmaStream) { + if (!motor->timerHardware || !motor->timerHardware->dmaStream) { return; } diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 30ccd983e..b89849325 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -58,7 +58,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value) motorDmaOutput_t * const motor = &dmaMotors[index]; - if (!motor->timerHardware->dmaStream) { + if (!motor->timerHardware || !motor->timerHardware->dmaStream) { return; } From 3c20108a997fe084d86daf7bef46aeabd77eb414 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 30 Jan 2017 22:17:01 +1100 Subject: [PATCH 22/55] Performance improvement based on feedback from @martinbudden --- src/main/drivers/pwm_output.c | 62 ++++++++++++++++++----------------- src/main/drivers/pwm_output.h | 1 + src/main/drivers/timer.c | 4 +++ 3 files changed, 37 insertions(+), 30 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index befbbedee..8f99c16ad 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -108,6 +108,12 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard *port->ccr = 0; } +static void pwmWriteUnused(uint8_t index, uint16_t value) +{ + UNUSED(index); + UNUSED(value); +} + static void pwmWriteBrushed(uint8_t index, uint16_t value) { *motors[index].ccr = (value - 1000) * motors[index].period / 1000; @@ -135,9 +141,7 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index].enabled) { - pwmWritePtr(index, value); - } + pwmWritePtr(index, value); } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) @@ -158,7 +162,8 @@ void pwmDisableMotors(void) void pwmEnableMotors(void) { - pwmMotorsEnabled = true; + /* check motors can be enabled */ + pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused); } bool pwmAreMotorsEnabled(void) @@ -166,23 +171,15 @@ bool pwmAreMotorsEnabled(void) return pwmMotorsEnabled; } +static void pwmCompleteWriteUnused(uint8_t motorCount) +{ + UNUSED(motorCount); +} + static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { for (int index = 0; index < motorCount; index++) { - - if (!motors[index].enabled) { - continue; - } - - bool overflowed = false; - // If we have not already overflowed this timer - for (int j = 0; j < index; j++) { - if (motors[j].tim == motors[index].tim) { - overflowed = true; - break; - } - } - if (!overflowed) { + if (motors[index].forceOverflow) { timerForceOverflow(motors[index].tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. @@ -193,9 +190,7 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) void pwmCompleteMotorUpdate(uint8_t motorCount) { - if (pwmCompleteWritePtr) { - pwmCompleteWritePtr(motorCount); - } + pwmCompleteWritePtr(motorCount); } void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) @@ -244,22 +239,20 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot #endif } - if (!useUnsyncedPwm && !isDigital) { - pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate; + if (!isDigital) { + pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate; } for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { const ioTag_t tag = motorConfig->ioTags[motorIndex]; - - if (!tag) { - break; - } - const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY); if (timerHardware == NULL) { - /* flag failure and disable ability to arm */ - break; + /* not enough motors initialised for the mixer or a break in the motors */ + pwmWritePtr = pwmWriteUnused; + pwmCompleteWritePtr = pwmCompleteWriteUnused; + /* TODO: block arming and add reason system cannot arm */ + return; } motors[motorIndex].io = IOGetByTag(tag); @@ -285,6 +278,15 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot } else { pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0); } + + bool timerAlreadyUsed = false; + for (int i = 0; i < motorIndex; i++) { + if (motors[i].tim == motors[motorIndex].tim) { + timerAlreadyUsed = true; + break; + } + } + motors[motorIndex].forceOverflow = !timerAlreadyUsed; motors[motorIndex].enabled = true; } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index a6ad7f38b..7102e7e35 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -105,6 +105,7 @@ typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointe typedef struct { volatile timCCR_t *ccr; TIM_TypeDef *tim; + bool forceOverflow; uint16_t period; bool enabled; IO_t io; diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 77067b3be..562993f2a 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -769,6 +769,10 @@ void timerForceOverflow(TIM_TypeDef *tim) const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) { + if (!tag) { + return NULL; + } + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { if (timerHardware[i].tag == tag) { if (timerHardware[i].usageFlags & flag || flag == 0) { From 605066ecf55b70a6667b7b406b2b50a4b4080748 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 30 Jan 2017 08:47:53 +1300 Subject: [PATCH 23/55] Fix rounding of float parameters when sent over MSP. --- src/main/fc/fc_msp.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 332f24eb8..404a89546 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1140,7 +1140,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, motorConfig()->useUnsyncedPwm); sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); - sbufWriteU16(dst, (uint16_t)(motorConfig()->digitalIdleOffsetPercent * 100)); + sbufWriteU16(dst, (uint16_t)lrintf(motorConfig()->digitalIdleOffsetPercent * 100)); sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); //!!TODO gyro_isr_update to be added pending decision //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); @@ -1169,8 +1169,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved - sbufWriteU16(dst, currentProfile->pidProfile.rateAccelLimit * 10); - sbufWriteU16(dst, currentProfile->pidProfile.yawRateAccelLimit * 10); + sbufWriteU16(dst, (uint16_t)lrintf(currentProfile->pidProfile.rateAccelLimit * 10)); + sbufWriteU16(dst, (uint16_t)lrintf(currentProfile->pidProfile.yawRateAccelLimit * 10)); sbufWriteU8(dst, currentProfile->pidProfile.levelAngleLimit); sbufWriteU8(dst, currentProfile->pidProfile.levelSensitivity); break; From 3bcf96e6ebd0cf8dd02f68b89569855186340e0a Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 30 Jan 2017 22:07:02 +0900 Subject: [PATCH 24/55] Manage BLINK attribute separate from config vars --- src/main/fc/cli.c | 42 ++++++++++++++++----------------- src/main/io/osd.c | 60 +++++++++++++++++++++++++++-------------------- src/main/io/osd.h | 4 +--- 3 files changed, 57 insertions(+), 49 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 022dff6de..a9403b462 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -774,27 +774,27 @@ const clivalue_t valueTable[] = { { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->time_alarm, .config.minmax = { 0, 60 } }, { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &osdProfile()->alt_alarm, .config.minmax = { 0, 10000 } }, - { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_horizon_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_current_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PIDRATE_PROFILE], .config.minmax = { 0, OSD_POS_MAX } }, - { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_WARNING], .config.minmax = { 0, OSD_POS_MAX } }, + { "osd_vbat_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYTIME], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_ontimer_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ONTIME], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_FLYMODE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_crosshairs", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CROSSHAIRS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_horizon_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_current_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SPEED], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_GPS_SATS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ALTITUDE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_ROLL_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pid_pitch_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PITCH_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pid_yaw_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_YAW_PIDS], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_POWER], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_PIDRATE_PROFILE], .config.minmax = { 0, OSD_POSCFG_MAX } }, + { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, &osdProfile()->item_pos[OSD_MAIN_BATT_WARNING], .config.minmax = { 0, OSD_POSCFG_MAX } }, #endif #ifdef USE_MAX7456 { "vcd_video_system", VAR_UINT8 | MASTER_VALUE, &vcdProfile()->video_system, .config.minmax = { 0, 2 } }, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4e94c8436..9f494ecb5 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -68,20 +68,28 @@ #define VIDEO_BUFFER_CHARS_PAL 480 -// Character coordinate and attributes +// Character coordinate #define OSD_POS(x,y) (x | (y << 5)) #define OSD_X(x) (x & 0x001F) #define OSD_Y(x) ((x >> 5) & 0x001F) +// Blink control + +bool blinkState = true; + +static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32]; +#define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32))) +#define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32))) +#define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32))) +#define BLINK(item) (IS_BLINK(item) && blinkState) + // Things in both OSD and CMS #define IS_HI(X) (rcData[X] > 1750) #define IS_LO(X) (rcData[X] < 1250) #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -bool blinkState = true; - //extern uint8_t RSSI; // TODO: not used? static uint16_t flyTime = 0; @@ -139,7 +147,7 @@ static int32_t osdGetAltitude(int32_t alt) static void osdDrawSingleElement(uint8_t item) { - if (!VISIBLE(osdProfile()->item_pos[item]) || BLINK(osdProfile()->item_pos[item])) + if (!VISIBLE(osdProfile()->item_pos[item]) || BLINK(item)) return; uint8_t elemPosX = OSD_X(osdProfile()->item_pos[item]); @@ -487,6 +495,8 @@ void osdInit(displayPort_t *osdDisplayPortToUse) armState = ARMING_FLAG(ARMED); + memset(blinkBits, 0, sizeof(blinkBits)); + displayClearScreen(osdDisplayPort); // display logo and help @@ -523,48 +533,48 @@ void osdUpdateAlarms(void) statRssi = rssi * 100 / 1024; if (statRssi < pOsdProfile->rssi_alarm) - pOsdProfile->item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + SET_BLINK(OSD_RSSI_VALUE); else - pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + CLR_BLINK(OSD_RSSI_VALUE); if (getVbat() <= (batteryWarningVoltage - 1)) { - pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; - pOsdProfile->item_pos[OSD_MAIN_BATT_WARNING] |= BLINK_FLAG; + SET_BLINK(OSD_MAIN_BATT_VOLTAGE); + SET_BLINK(OSD_MAIN_BATT_WARNING); } else { - pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - pOsdProfile->item_pos[OSD_MAIN_BATT_WARNING] &= ~BLINK_FLAG; + CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); + CLR_BLINK(OSD_MAIN_BATT_WARNING); } if (STATE(GPS_FIX) == 0) - pOsdProfile->item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + SET_BLINK(OSD_GPS_SATS); else - pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + CLR_BLINK(OSD_GPS_SATS); if (flyTime / 60 >= pOsdProfile->time_alarm && ARMING_FLAG(ARMED)) - pOsdProfile->item_pos[OSD_FLYTIME] |= BLINK_FLAG; + SET_BLINK(OSD_FLYTIME); else - pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + CLR_BLINK(OSD_FLYTIME); if (mAhDrawn >= pOsdProfile->cap_alarm) - pOsdProfile->item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + SET_BLINK(OSD_MAH_DRAWN); else - pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + CLR_BLINK(OSD_MAH_DRAWN); if (alt >= pOsdProfile->alt_alarm) - pOsdProfile->item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + SET_BLINK(OSD_ALTITUDE); else - pOsdProfile->item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; + CLR_BLINK(OSD_ALTITUDE); } void osdResetAlarms(void) { - osd_profile_t *pOsdProfile = &masterConfig.osdProfile; - - pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - pOsdProfile->item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - pOsdProfile->item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - pOsdProfile->item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + CLR_BLINK(OSD_RSSI_VALUE); + CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); + CLR_BLINK(OSD_MAIN_BATT_WARNING); + CLR_BLINK(OSD_GPS_SATS); + CLR_BLINK(OSD_FLYTIME); + CLR_BLINK(OSD_MAH_DRAWN); + CLR_BLINK(OSD_ALTITUDE); } static void osdResetStats(void) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 756aa0d1c..c6539b42d 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -20,11 +20,9 @@ #include "common/time.h" #define VISIBLE_FLAG 0x0800 -#define BLINK_FLAG 0x0400 #define VISIBLE(x) (x & VISIBLE_FLAG) -#define BLINK(x) ((x & BLINK_FLAG) && blinkState) -#define BLINK_OFF(x) (x & ~BLINK_FLAG) #define OSD_POS_MAX 0x3FF +#define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values typedef enum { OSD_RSSI_VALUE, From 7bfd2b5d1c67d1572050ebaabb16c6896cc0c6e3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 16:09:58 +0100 Subject: [PATCH 25/55] Get rid of unneeded constrain --- src/main/flight/pid.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 72a2adf6e..b580496b5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -254,8 +254,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an dynKi *= itermAccelerator; float ITermDelta = Ki[axis] * errorRate * dT * dynKi; ITerm += ITermDelta; - // also limit maximum integrator value to prevent windup - ITerm = constrainf(ITerm, -250.0f, 250.0f); previousGyroIf[axis] = ITerm; } From 98f8a4d59e1e951ca773e9931ca1b6e2f693cde1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 17:01:54 +0100 Subject: [PATCH 26/55] Add KISSCC (KISSFC alias) --- src/main/target/KISSFC/KISSCC.mk | 0 src/main/target/KISSFC/target.c | 14 ++++++++++++++ src/main/target/KISSFC/target.h | 20 ++++++++++++++++++++ 3 files changed, 34 insertions(+) create mode 100755 src/main/target/KISSFC/KISSCC.mk diff --git a/src/main/target/KISSFC/KISSCC.mk b/src/main/target/KISSFC/KISSCC.mk new file mode 100755 index 000000000..e69de29bb diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index a48fc93a0..0832a9900 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -25,6 +25,19 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { +#ifdef KISSCC + DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM17,CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED), + DEF_TIM(TIM16,CH1N,PA13, TIM_USE_PWM, TIMER_INPUT_ENABLED), // KISSCC new softserial? +#else DEF_TIM(TIM1, CH2N,PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), DEF_TIM(TIM8, CH2N,PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), DEF_TIM(TIM15,CH1N,PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), @@ -35,4 +48,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, TIMER_INPUT_ENABLED), DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED), DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, TIMER_INPUT_ENABLED), +#endif }; diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 90e32cf6d..4960cb30e 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -47,11 +47,17 @@ #define USE_ACC_MPU6050 #define ACC_MPU6050_ALIGN CW180_DEG +#define USE_SOFTSERIAL #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 +#ifdef KISSCC +#define USE_SOFTSERIAL1 +#define SERIAL_PORT_COUNT 5 +#else #define SERIAL_PORT_COUNT 4 +#endif #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -62,6 +68,12 @@ #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) +#ifdef KISSCC +#define SOFTSERIAL_1_TIMER TIM16 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 11 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 11 +#endif + #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA @@ -72,7 +84,11 @@ //#define CURRENT_METER_ADC_PIN PA5 //#define RSSI_ADC_PIN PB2 +#ifdef KISSCC +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#else #define DEFAULT_FEATURES FEATURE_VBAT +#endif #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 @@ -88,5 +104,9 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#ifdef KISSCC +#define USABLE_TIMER_CHANNEL_COUNT 11 +#else #define USABLE_TIMER_CHANNEL_COUNT 10 +#endif #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) From 3d8c098bd8cc38b3039ff2117413917319dcb7aa Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 20:58:58 +0100 Subject: [PATCH 27/55] Separate fc_core.c from RC processing --- Makefile | 3 +- src/main/fc/fc_core.c | 241 +---------------------------------- src/main/fc/fc_core.h | 4 +- src/main/fc/fc_rc.c | 289 ++++++++++++++++++++++++++++++++++++++++++ src/main/fc/fc_rc.h | 31 +++++ src/main/flight/pid.c | 7 +- src/main/flight/pid.h | 2 +- 7 files changed, 332 insertions(+), 245 deletions(-) create mode 100755 src/main/fc/fc_rc.c create mode 100755 src/main/fc/fc_rc.h diff --git a/Makefile b/Makefile index 0381e7412..56c02912f 100644 --- a/Makefile +++ b/Makefile @@ -590,6 +590,7 @@ COMMON_SRC = \ fc/fc_dispatch.c \ fc/fc_hardfaults.c \ fc/fc_core.c \ + fc/fc_rc.c \ fc/fc_msp.c \ fc/fc_tasks.c \ fc/rc_controls.c \ @@ -711,7 +712,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/system.c \ drivers/timer.c \ fc/fc_tasks.c \ - fc/mw.c \ + fc/fc_rc.c \ fc/rc_controls.c \ fc/rc_curves.c \ fc/runtime_config.c \ diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f1505c370..f4c8acdc4 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -44,6 +44,7 @@ #include "fc/rc_curves.h" #include "fc/runtime_config.h" #include "fc/cli.h" +#include "fc/fc_rc.h" #include "msp/msp_serial.h" @@ -90,23 +91,8 @@ uint8_t motorControlEnable = false; int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero -static float throttlePIDAttenuation; - bool isRXDataNew; static bool armingCalibrationWasInitialised; -static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; - -float getSetpointRate(int axis) { - return setpointRate[axis]; -} - -float getRcDeflection(int axis) { - return rcDeflection[axis]; -} - -float getRcDeflectionAbs(int axis) { - return rcDeflectionAbs[axis]; -} void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { @@ -129,226 +115,6 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -#define SETPOINT_RATE_LIMIT 1998.0f -#define RC_RATE_INCREMENTAL 14.54f - -void calculateSetpointRate(int axis, int16_t rc) { - float angleRate, rcRate, rcSuperfactor, rcCommandf; - uint8_t rcExpo; - - if (axis != YAW) { - rcExpo = currentControlRateProfile->rcExpo8; - rcRate = currentControlRateProfile->rcRate8 / 100.0f; - } else { - rcExpo = currentControlRateProfile->rcYawExpo8; - rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; - } - - if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); - rcCommandf = rc / 500.0f; - rcDeflection[axis] = rcCommandf; - rcDeflectionAbs[axis] = ABS(rcCommandf); - - if (rcExpo) { - float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof); - } - - angleRate = 200.0f * rcRate * rcCommandf; - - if (currentControlRateProfile->rates[axis]) { - rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - angleRate *= rcSuperfactor; - } - - DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); - - setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) -} - -void scaleRcCommandToFpvCamAngle(void) { - //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed - static uint8_t lastFpvCamAngleDegrees = 0; - static float cosFactor = 1.0; - static float sinFactor = 0.0; - - if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){ - lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; - cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); - sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); - } - - float roll = setpointRate[ROLL]; - float yaw = setpointRate[YAW]; - setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); - setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); -} - -#define THROTTLE_BUFFER_MAX 20 -#define THROTTLE_DELTA_MS 100 - - 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)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold; - - rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; - if (index >= indexMax) - index = 0; - - const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - - if(ABS(rcCommandSpeed) > throttleVelocityThreshold) - pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain); - else - pidSetItermAccelerator(1.0f); -} - -void processRcCommand(void) -{ - static int16_t lastCommand[4] = { 0, 0, 0, 0 }; - static int16_t deltaRC[4] = { 0, 0, 0, 0 }; - static int16_t factor, rcInterpolationFactor; - static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; - uint16_t rxRefreshRate; - bool readyToCalculateRate = false; - uint8_t readyToCalculateRateAxisCnt = 0; - - if (isRXDataNew) { - currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); - checkForThrottleErrorResetState(currentRxRefreshRate); - } - - if (rxConfig()->rcInterpolation || flightModeFlags) { - // 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(); - } - - if (isRXDataNew) { - rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; - - if (debugMode == DEBUG_RC_INTERPOLATION) { - for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; - debug[3] = rxRefreshRate; - } - - for (int channel=ROLL; channel < interpolationChannels; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; - } - - factor = rcInterpolationFactor - 1; - } else { - factor--; - } - - // Interpolate steps of rcCommand - if (factor > 0) { - for (int channel=ROLL; channel < interpolationChannels; channel++) { - rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation - readyToCalculateRate = true; - } - } else { - factor = 0; - } - } else { - factor = 0; // reset factor in case of level modes flip flopping - } - - if (readyToCalculateRate || isRXDataNew) { - if (isRXDataNew) - readyToCalculateRateAxisCnt = FD_YAW; - - for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) - calculateSetpointRate(axis, rcCommand[axis]); - - // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) - if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) - scaleRcCommandToFpvCamAngle(); - - isRXDataNew = false; - } -} - -void updateRcCommands(void) -{ - // PITCH & ROLL only dynamic PID adjustment, depending on throttle value - int32_t prop; - if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { - prop = 100; - throttlePIDAttenuation = 1.0f; - } else { - if (rcData[THROTTLE] < 2000) { - prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); - } else { - prop = 100 - currentControlRateProfile->dynThrPID; - } - throttlePIDAttenuation = prop / 100.0f; - } - - for (int axis = 0; axis < 3; axis++) { - // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. - - int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); - if (axis == ROLL || axis == PITCH) { - if (tmp > rcControlsConfig()->deadband) { - tmp -= rcControlsConfig()->deadband; - } else { - tmp = 0; - } - rcCommand[axis] = tmp; - } else { - if (tmp > rcControlsConfig()->yaw_deadband) { - tmp -= rcControlsConfig()->yaw_deadband; - } else { - tmp = 0; - } - rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction; - } - if (rcData[axis] < rxConfig()->midrc) { - rcCommand[axis] = -rcCommand[axis]; - } - } - - int32_t tmp; - if (feature(FEATURE_3D)) { - tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - PWM_RANGE_MIN); - } else { - tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); - } - rcCommand[THROTTLE] = rcLookupThrottle(tmp); - - if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { - fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); - rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); - } - - if (FLIGHT_MODE(HEADFREE_MODE)) { - const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); - const float cosDiff = cos_approx(radDiff); - const float sinDiff = sin_approx(radDiff); - const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; - rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; - rcCommand[PITCH] = rcCommand_PITCH; - } -} - void updateLEDs(void) { if (ARMING_FLAG(ARMED)) { @@ -698,7 +464,7 @@ static void subTaskPidController(void) uint32_t startTime; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // PID - note this is function pointer set by setPIDController() - pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation); + pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); } @@ -741,8 +507,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { - rcCommand[YAW] = 0; - setpointRate[YAW] = 0; + resetYawAxis(); } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 000e9844a..5e2766d60 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -21,6 +21,7 @@ extern int16_t magHold; extern bool isRXDataNew; +extern int16_t headFreeModeHold; union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); @@ -34,6 +35,3 @@ void updateLEDs(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); -float getSetpointRate(int axis); -float getRcDeflection(int axis); -float getRcDeflectionAbs(int axis); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c new file mode 100755 index 000000000..ebf3fb34d --- /dev/null +++ b/src/main/fc/fc_rc.c @@ -0,0 +1,289 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/maths.h" +#include "common/axis.h" +#include "common/utils.h" +#include "common/filter.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "fc/runtime_config.h" +#include "fc/fc_rc.h" +#include "fc/fc_core.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "flight/pid.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; +static float throttlePIDAttenuation; + +float getSetpointRate(int axis) { + return setpointRate[axis]; +} + +float getRcDeflection(int axis) { + return rcDeflection[axis]; +} + +float getRcDeflectionAbs(int axis) { + return rcDeflectionAbs[axis]; +} + +float getThrottlePIDAttenuation(void) { + return throttlePIDAttenuation; +} + +#define SETPOINT_RATE_LIMIT 1998.0f +#define RC_RATE_INCREMENTAL 14.54f + +void calculateSetpointRate(int axis, int16_t rc) { + float angleRate, rcRate, rcSuperfactor, rcCommandf; + uint8_t rcExpo; + + if (axis != YAW) { + rcExpo = currentControlRateProfile->rcExpo8; + rcRate = currentControlRateProfile->rcRate8 / 100.0f; + } else { + rcExpo = currentControlRateProfile->rcYawExpo8; + rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; + } + + if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); + rcCommandf = rc / 500.0f; + rcDeflection[axis] = rcCommandf; + rcDeflectionAbs[axis] = ABS(rcCommandf); + + if (rcExpo) { + float expof = rcExpo / 100.0f; + rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof); + } + + angleRate = 200.0f * rcRate * rcCommandf; + + if (currentControlRateProfile->rates[axis]) { + rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + angleRate *= rcSuperfactor; + } + + DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); + + setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) +} + +void scaleRcCommandToFpvCamAngle(void) { + //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed + static uint8_t lastFpvCamAngleDegrees = 0; + static float cosFactor = 1.0; + static float sinFactor = 0.0; + + if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){ + lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; + cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); + sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); + } + + float roll = setpointRate[ROLL]; + float yaw = setpointRate[YAW]; + setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); +} + +#define THROTTLE_BUFFER_MAX 20 +#define THROTTLE_DELTA_MS 100 + + 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)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold; + + rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; + if (index >= indexMax) + index = 0; + + const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; + + if(ABS(rcCommandSpeed) > throttleVelocityThreshold) + pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain); + else + pidSetItermAccelerator(1.0f); +} + +void processRcCommand(void) +{ + static int16_t lastCommand[4] = { 0, 0, 0, 0 }; + static int16_t deltaRC[4] = { 0, 0, 0, 0 }; + static int16_t factor, rcInterpolationFactor; + static uint16_t currentRxRefreshRate; + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; + uint16_t rxRefreshRate; + bool readyToCalculateRate = false; + uint8_t readyToCalculateRateAxisCnt = 0; + + if (isRXDataNew) { + currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); + checkForThrottleErrorResetState(currentRxRefreshRate); + } + + if (rxConfig()->rcInterpolation || flightModeFlags) { + // 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(); + } + + if (isRXDataNew) { + rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; + + if (debugMode == DEBUG_RC_INTERPOLATION) { + for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; + debug[3] = rxRefreshRate; + } + + for (int channel=ROLL; channel < interpolationChannels; channel++) { + deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); + lastCommand[channel] = rcCommand[channel]; + } + + factor = rcInterpolationFactor - 1; + } else { + factor--; + } + + // Interpolate steps of rcCommand + if (factor > 0) { + for (int channel=ROLL; channel < interpolationChannels; channel++) { + rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; + readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation + readyToCalculateRate = true; + } + } else { + factor = 0; + } + } else { + factor = 0; // reset factor in case of level modes flip flopping + } + + if (readyToCalculateRate || isRXDataNew) { + if (isRXDataNew) + readyToCalculateRateAxisCnt = FD_YAW; + + for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) + calculateSetpointRate(axis, rcCommand[axis]); + + // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) + if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) + scaleRcCommandToFpvCamAngle(); + + isRXDataNew = false; + } +} + +void updateRcCommands(void) +{ + // PITCH & ROLL only dynamic PID adjustment, depending on throttle value + int32_t prop; + if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { + prop = 100; + throttlePIDAttenuation = 1.0f; + } else { + if (rcData[THROTTLE] < 2000) { + prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); + } else { + prop = 100 - currentControlRateProfile->dynThrPID; + } + throttlePIDAttenuation = prop / 100.0f; + } + + for (int axis = 0; axis < 3; axis++) { + // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. + + int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); + if (axis == ROLL || axis == PITCH) { + if (tmp > rcControlsConfig()->deadband) { + tmp -= rcControlsConfig()->deadband; + } else { + tmp = 0; + } + rcCommand[axis] = tmp; + } else { + if (tmp > rcControlsConfig()->yaw_deadband) { + tmp -= rcControlsConfig()->yaw_deadband; + } else { + tmp = 0; + } + rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction; + } + if (rcData[axis] < rxConfig()->midrc) { + rcCommand[axis] = -rcCommand[axis]; + } + } + + int32_t tmp; + if (feature(FEATURE_3D)) { + tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); + tmp = (uint32_t)(tmp - PWM_RANGE_MIN); + } else { + tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); + tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); + } + rcCommand[THROTTLE] = rcLookupThrottle(tmp); + + if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + } + + if (FLIGHT_MODE(HEADFREE_MODE)) { + const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); + const float cosDiff = cos_approx(radDiff); + const float sinDiff = sin_approx(radDiff); + const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; + rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; + rcCommand[PITCH] = rcCommand_PITCH; + } +} + +void resetYawAxis(void) { + rcCommand[YAW] = 0; + setpointRate[YAW] = 0; +} diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h new file mode 100755 index 000000000..34114b053 --- /dev/null +++ b/src/main/fc/fc_rc.h @@ -0,0 +1,31 @@ +/* + * 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 + +void calculateSetpointRate(int axis, int16_t rc); +void scaleRcCommandToFpvCamAngle(void); +void checkForThrottleErrorResetState(uint16_t rxRefreshRate); +void checkForThrottleErrorResetState(uint16_t rxRefreshRate); +void processRcCommand(void); +float getSetpointRate(int axis); +float getRcDeflection(int axis); +float getRcDeflectionAbs(int axis); +float getThrottlePIDAttenuation(void); +void updateRcCommands(void); +void resetYawAxis(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b580496b5..bcbdbaaaa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -29,6 +29,8 @@ #include "common/filter.h" #include "fc/fc_core.h" +#include "fc/fc_rc.h" + #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -211,11 +213,12 @@ static float accelerationLimit(int axis, float currentPidSetpoint) { // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) -void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor) +void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim) { static float previousRateError[2]; - + const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); + // Dynamic ki component to gradually scale back integration when above windup point float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6162d25e3..2c40241d3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -90,7 +90,7 @@ typedef struct pidConfig_s { } pidConfig_t; union rollAndPitchTrims_u; -void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor); +void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim); extern float axisPIDf[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; From a112c1d7d0a4597504fd7e4da214b35e39320937 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 30 Jan 2017 20:58:58 +0100 Subject: [PATCH 28/55] Separate fc_core.c from RC processing --- Makefile | 3 +- src/main/fc/fc_core.c | 241 +---------------------------------- src/main/fc/fc_core.h | 4 +- src/main/fc/fc_rc.c | 289 ++++++++++++++++++++++++++++++++++++++++++ src/main/fc/fc_rc.h | 31 +++++ src/main/flight/pid.c | 7 +- src/main/flight/pid.h | 2 +- 7 files changed, 332 insertions(+), 245 deletions(-) create mode 100755 src/main/fc/fc_rc.c create mode 100755 src/main/fc/fc_rc.h diff --git a/Makefile b/Makefile index 0381e7412..56c02912f 100644 --- a/Makefile +++ b/Makefile @@ -590,6 +590,7 @@ COMMON_SRC = \ fc/fc_dispatch.c \ fc/fc_hardfaults.c \ fc/fc_core.c \ + fc/fc_rc.c \ fc/fc_msp.c \ fc/fc_tasks.c \ fc/rc_controls.c \ @@ -711,7 +712,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/system.c \ drivers/timer.c \ fc/fc_tasks.c \ - fc/mw.c \ + fc/fc_rc.c \ fc/rc_controls.c \ fc/rc_curves.c \ fc/runtime_config.c \ diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f1505c370..94897ca30 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -44,6 +44,7 @@ #include "fc/rc_curves.h" #include "fc/runtime_config.h" #include "fc/cli.h" +#include "fc/fc_rc.h" #include "msp/msp_serial.h" @@ -90,23 +91,8 @@ uint8_t motorControlEnable = false; int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero -static float throttlePIDAttenuation; - bool isRXDataNew; static bool armingCalibrationWasInitialised; -static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; - -float getSetpointRate(int axis) { - return setpointRate[axis]; -} - -float getRcDeflection(int axis) { - return rcDeflection[axis]; -} - -float getRcDeflectionAbs(int axis) { - return rcDeflectionAbs[axis]; -} void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { @@ -129,226 +115,6 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -#define SETPOINT_RATE_LIMIT 1998.0f -#define RC_RATE_INCREMENTAL 14.54f - -void calculateSetpointRate(int axis, int16_t rc) { - float angleRate, rcRate, rcSuperfactor, rcCommandf; - uint8_t rcExpo; - - if (axis != YAW) { - rcExpo = currentControlRateProfile->rcExpo8; - rcRate = currentControlRateProfile->rcRate8 / 100.0f; - } else { - rcExpo = currentControlRateProfile->rcYawExpo8; - rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; - } - - if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); - rcCommandf = rc / 500.0f; - rcDeflection[axis] = rcCommandf; - rcDeflectionAbs[axis] = ABS(rcCommandf); - - if (rcExpo) { - float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof); - } - - angleRate = 200.0f * rcRate * rcCommandf; - - if (currentControlRateProfile->rates[axis]) { - rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - angleRate *= rcSuperfactor; - } - - DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); - - setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) -} - -void scaleRcCommandToFpvCamAngle(void) { - //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed - static uint8_t lastFpvCamAngleDegrees = 0; - static float cosFactor = 1.0; - static float sinFactor = 0.0; - - if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){ - lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; - cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); - sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); - } - - float roll = setpointRate[ROLL]; - float yaw = setpointRate[YAW]; - setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); - setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); -} - -#define THROTTLE_BUFFER_MAX 20 -#define THROTTLE_DELTA_MS 100 - - 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)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold; - - rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; - if (index >= indexMax) - index = 0; - - const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - - if(ABS(rcCommandSpeed) > throttleVelocityThreshold) - pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain); - else - pidSetItermAccelerator(1.0f); -} - -void processRcCommand(void) -{ - static int16_t lastCommand[4] = { 0, 0, 0, 0 }; - static int16_t deltaRC[4] = { 0, 0, 0, 0 }; - static int16_t factor, rcInterpolationFactor; - static uint16_t currentRxRefreshRate; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; - uint16_t rxRefreshRate; - bool readyToCalculateRate = false; - uint8_t readyToCalculateRateAxisCnt = 0; - - if (isRXDataNew) { - currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); - checkForThrottleErrorResetState(currentRxRefreshRate); - } - - if (rxConfig()->rcInterpolation || flightModeFlags) { - // 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(); - } - - if (isRXDataNew) { - rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; - - if (debugMode == DEBUG_RC_INTERPOLATION) { - for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; - debug[3] = rxRefreshRate; - } - - for (int channel=ROLL; channel < interpolationChannels; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; - } - - factor = rcInterpolationFactor - 1; - } else { - factor--; - } - - // Interpolate steps of rcCommand - if (factor > 0) { - for (int channel=ROLL; channel < interpolationChannels; channel++) { - rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation - readyToCalculateRate = true; - } - } else { - factor = 0; - } - } else { - factor = 0; // reset factor in case of level modes flip flopping - } - - if (readyToCalculateRate || isRXDataNew) { - if (isRXDataNew) - readyToCalculateRateAxisCnt = FD_YAW; - - for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) - calculateSetpointRate(axis, rcCommand[axis]); - - // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) - if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) - scaleRcCommandToFpvCamAngle(); - - isRXDataNew = false; - } -} - -void updateRcCommands(void) -{ - // PITCH & ROLL only dynamic PID adjustment, depending on throttle value - int32_t prop; - if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { - prop = 100; - throttlePIDAttenuation = 1.0f; - } else { - if (rcData[THROTTLE] < 2000) { - prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); - } else { - prop = 100 - currentControlRateProfile->dynThrPID; - } - throttlePIDAttenuation = prop / 100.0f; - } - - for (int axis = 0; axis < 3; axis++) { - // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. - - int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); - if (axis == ROLL || axis == PITCH) { - if (tmp > rcControlsConfig()->deadband) { - tmp -= rcControlsConfig()->deadband; - } else { - tmp = 0; - } - rcCommand[axis] = tmp; - } else { - if (tmp > rcControlsConfig()->yaw_deadband) { - tmp -= rcControlsConfig()->yaw_deadband; - } else { - tmp = 0; - } - rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction; - } - if (rcData[axis] < rxConfig()->midrc) { - rcCommand[axis] = -rcCommand[axis]; - } - } - - int32_t tmp; - if (feature(FEATURE_3D)) { - tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - PWM_RANGE_MIN); - } else { - tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); - } - rcCommand[THROTTLE] = rcLookupThrottle(tmp); - - if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { - fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); - rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); - } - - if (FLIGHT_MODE(HEADFREE_MODE)) { - const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); - const float cosDiff = cos_approx(radDiff); - const float sinDiff = sin_approx(radDiff); - const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; - rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; - rcCommand[PITCH] = rcCommand_PITCH; - } -} - void updateLEDs(void) { if (ARMING_FLAG(ARMED)) { @@ -698,7 +464,7 @@ static void subTaskPidController(void) uint32_t startTime; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // PID - note this is function pointer set by setPIDController() - pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims, throttlePIDAttenuation); + pidController(¤tProfile->pidProfile, &accelerometerConfig()->accelerometerTrims); DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); } @@ -741,8 +507,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { - rcCommand[YAW] = 0; - setpointRate[YAW] = 0; + resetYawAxis(); } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 000e9844a..5e2766d60 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -21,6 +21,7 @@ extern int16_t magHold; extern bool isRXDataNew; +extern int16_t headFreeModeHold; union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); @@ -34,6 +35,3 @@ void updateLEDs(void); void updateRcCommands(void); void taskMainPidLoop(timeUs_t currentTimeUs); -float getSetpointRate(int axis); -float getRcDeflection(int axis); -float getRcDeflectionAbs(int axis); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c new file mode 100755 index 000000000..ebf3fb34d --- /dev/null +++ b/src/main/fc/fc_rc.c @@ -0,0 +1,289 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/maths.h" +#include "common/axis.h" +#include "common/utils.h" +#include "common/filter.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "fc/runtime_config.h" +#include "fc/fc_rc.h" +#include "fc/fc_core.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "flight/pid.h" + +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; +static float throttlePIDAttenuation; + +float getSetpointRate(int axis) { + return setpointRate[axis]; +} + +float getRcDeflection(int axis) { + return rcDeflection[axis]; +} + +float getRcDeflectionAbs(int axis) { + return rcDeflectionAbs[axis]; +} + +float getThrottlePIDAttenuation(void) { + return throttlePIDAttenuation; +} + +#define SETPOINT_RATE_LIMIT 1998.0f +#define RC_RATE_INCREMENTAL 14.54f + +void calculateSetpointRate(int axis, int16_t rc) { + float angleRate, rcRate, rcSuperfactor, rcCommandf; + uint8_t rcExpo; + + if (axis != YAW) { + rcExpo = currentControlRateProfile->rcExpo8; + rcRate = currentControlRateProfile->rcRate8 / 100.0f; + } else { + rcExpo = currentControlRateProfile->rcYawExpo8; + rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; + } + + if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); + rcCommandf = rc / 500.0f; + rcDeflection[axis] = rcCommandf; + rcDeflectionAbs[axis] = ABS(rcCommandf); + + if (rcExpo) { + float expof = rcExpo / 100.0f; + rcCommandf = rcCommandf * power3(rcDeflectionAbs[axis]) * expof + rcCommandf * (1-expof); + } + + angleRate = 200.0f * rcRate * rcCommandf; + + if (currentControlRateProfile->rates[axis]) { + rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + angleRate *= rcSuperfactor; + } + + DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); + + setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec) +} + +void scaleRcCommandToFpvCamAngle(void) { + //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed + static uint8_t lastFpvCamAngleDegrees = 0; + static float cosFactor = 1.0; + static float sinFactor = 0.0; + + if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){ + lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; + cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); + sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); + } + + float roll = setpointRate[ROLL]; + float yaw = setpointRate[YAW]; + setpointRate[ROLL] = constrainf(roll * cosFactor - yaw * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + setpointRate[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); +} + +#define THROTTLE_BUFFER_MAX 20 +#define THROTTLE_DELTA_MS 100 + + 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)) ? currentProfile->pidProfile.itermThrottleThreshold / 2 : currentProfile->pidProfile.itermThrottleThreshold; + + rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; + if (index >= indexMax) + index = 0; + + const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; + + if(ABS(rcCommandSpeed) > throttleVelocityThreshold) + pidSetItermAccelerator(currentProfile->pidProfile.itermAcceleratorGain); + else + pidSetItermAccelerator(1.0f); +} + +void processRcCommand(void) +{ + static int16_t lastCommand[4] = { 0, 0, 0, 0 }; + static int16_t deltaRC[4] = { 0, 0, 0, 0 }; + static int16_t factor, rcInterpolationFactor; + static uint16_t currentRxRefreshRate; + const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; + uint16_t rxRefreshRate; + bool readyToCalculateRate = false; + uint8_t readyToCalculateRateAxisCnt = 0; + + if (isRXDataNew) { + currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); + checkForThrottleErrorResetState(currentRxRefreshRate); + } + + if (rxConfig()->rcInterpolation || flightModeFlags) { + // 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(); + } + + if (isRXDataNew) { + rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; + + if (debugMode == DEBUG_RC_INTERPOLATION) { + for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; + debug[3] = rxRefreshRate; + } + + for (int channel=ROLL; channel < interpolationChannels; channel++) { + deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); + lastCommand[channel] = rcCommand[channel]; + } + + factor = rcInterpolationFactor - 1; + } else { + factor--; + } + + // Interpolate steps of rcCommand + if (factor > 0) { + for (int channel=ROLL; channel < interpolationChannels; channel++) { + rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; + readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation + readyToCalculateRate = true; + } + } else { + factor = 0; + } + } else { + factor = 0; // reset factor in case of level modes flip flopping + } + + if (readyToCalculateRate || isRXDataNew) { + if (isRXDataNew) + readyToCalculateRateAxisCnt = FD_YAW; + + for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) + calculateSetpointRate(axis, rcCommand[axis]); + + // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) + if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) + scaleRcCommandToFpvCamAngle(); + + isRXDataNew = false; + } +} + +void updateRcCommands(void) +{ + // PITCH & ROLL only dynamic PID adjustment, depending on throttle value + int32_t prop; + if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { + prop = 100; + throttlePIDAttenuation = 1.0f; + } else { + if (rcData[THROTTLE] < 2000) { + prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); + } else { + prop = 100 - currentControlRateProfile->dynThrPID; + } + throttlePIDAttenuation = prop / 100.0f; + } + + for (int axis = 0; axis < 3; axis++) { + // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. + + int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); + if (axis == ROLL || axis == PITCH) { + if (tmp > rcControlsConfig()->deadband) { + tmp -= rcControlsConfig()->deadband; + } else { + tmp = 0; + } + rcCommand[axis] = tmp; + } else { + if (tmp > rcControlsConfig()->yaw_deadband) { + tmp -= rcControlsConfig()->yaw_deadband; + } else { + tmp = 0; + } + rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction; + } + if (rcData[axis] < rxConfig()->midrc) { + rcCommand[axis] = -rcCommand[axis]; + } + } + + int32_t tmp; + if (feature(FEATURE_3D)) { + tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); + tmp = (uint32_t)(tmp - PWM_RANGE_MIN); + } else { + tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); + tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); + } + rcCommand[THROTTLE] = rcLookupThrottle(tmp); + + if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + } + + if (FLIGHT_MODE(HEADFREE_MODE)) { + const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); + const float cosDiff = cos_approx(radDiff); + const float sinDiff = sin_approx(radDiff); + const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; + rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; + rcCommand[PITCH] = rcCommand_PITCH; + } +} + +void resetYawAxis(void) { + rcCommand[YAW] = 0; + setpointRate[YAW] = 0; +} diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h new file mode 100755 index 000000000..34114b053 --- /dev/null +++ b/src/main/fc/fc_rc.h @@ -0,0 +1,31 @@ +/* + * 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 + +void calculateSetpointRate(int axis, int16_t rc); +void scaleRcCommandToFpvCamAngle(void); +void checkForThrottleErrorResetState(uint16_t rxRefreshRate); +void checkForThrottleErrorResetState(uint16_t rxRefreshRate); +void processRcCommand(void); +float getSetpointRate(int axis); +float getRcDeflection(int axis); +float getRcDeflectionAbs(int axis); +float getThrottlePIDAttenuation(void); +void updateRcCommands(void); +void resetYawAxis(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b580496b5..bcbdbaaaa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -29,6 +29,8 @@ #include "common/filter.h" #include "fc/fc_core.h" +#include "fc/fc_rc.h" + #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -211,11 +213,12 @@ static float accelerationLimit(int axis, float currentPidSetpoint) { // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) -void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor) +void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim) { static float previousRateError[2]; - + const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); + // Dynamic ki component to gradually scale back integration when above windup point float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6162d25e3..2c40241d3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -90,7 +90,7 @@ typedef struct pidConfig_s { } pidConfig_t; union rollAndPitchTrims_u; -void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, float tpaFactor); +void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim); extern float axisPIDf[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; From d3759067429fa0a46972e51466e4c98b316b16e7 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 31 Jan 2017 11:52:49 +1300 Subject: [PATCH 29/55] Fixed bug causing 'map' to be shown in 'diff' even if it was default. --- src/main/fc/cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a9403b462..c5a882242 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2703,7 +2703,7 @@ static void cliBeeper(char *cmdline) static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) { - bool equalsDefault = false; + bool equalsDefault = true; char buf[16]; char bufDefault[16]; uint32_t i; From 4364fcb2304ef06afb66d9ab8696afd0dfff9140 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 31 Jan 2017 10:00:24 +1100 Subject: [PATCH 30/55] Allow resources to be reassigned (when in the same resource group) Minor improvement to allow index for non-indexable settings, such that duplicate code removed for DIFF. --- src/main/fc/cli.c | 110 +++++++++++++++++++++------------------------- 1 file changed, 49 insertions(+), 61 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a9403b462..1fff9f922 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3315,6 +3315,8 @@ static void cliVersion(char *cmdline) #if defined(USE_RESOURCE_MGMT) +#define MAX_RESOURCE_INDEX(x) (x == 0 ? 1 : x) + typedef struct { const uint8_t owner; ioTag_t *ptr; @@ -3348,76 +3350,55 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig) const char* owner; owner = ownerNames[resourceTable[i].owner]; - if (resourceTable[i].maxIndex > 0) { - for (int index = 0; index < resourceTable[i].maxIndex; index++) { - ioTag_t ioTag = *(resourceTable[i].ptr + index); - ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); - - bool equalsDefault = ioTag == ioTagDefault; - const char *format = "resource %s %d %c%02d\r\n"; - const char *formatUnassigned = "resource %s %d NONE\r\n"; - if (!ioTagDefault) { - cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); - } else { - cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); - } - if (!ioTag) { - if (!(dumpMask & HIDE_UNUSED)) { - cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); - } - } else { - cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); - } - } - } else { - ioTag_t ioTag = *resourceTable[i].ptr; - ioTag_t ioTagDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig); + for (int index = 0; index < MAX_RESOURCE_INDEX(resourceTable[i].maxIndex); index++) { + ioTag_t ioTag = *(resourceTable[i].ptr + index); + ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig); bool equalsDefault = ioTag == ioTagDefault; - const char *format = "resource %s %c%02d\r\n"; - const char *formatUnassigned = "resource %s NONE\r\n"; + const char *format = "resource %s %d %c%02d\r\n"; + const char *formatUnassigned = "resource %s %d NONE\r\n"; if (!ioTagDefault) { - cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); } else { - cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); + cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault)); } if (!ioTag) { if (!(dumpMask & HIDE_UNUSED)) { - cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner); + cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index)); } } else { - cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); + cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag)); } } } } -static bool resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) +static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { - const char * format = "\r\n* %s * %c%d also used by %s"; + const char * format = "\r\n* WARNING * %c%d also used by %s"; for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { - for (int i = 0; i < (resourceTable[r].maxIndex == 0 ? 1 : resourceTable[r].maxIndex); i++) { + for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) { if (*(resourceTable[r].ptr + i) == tag) { - bool error = false; + bool cleared = false; if (r == resourceIndex) { if (i == index) { continue; } - error = true; + *(resourceTable[r].ptr + i) = IO_TAG_NONE; + cleared = true; } - cliPrintf(format, error ? "ERROR" : "WARNING", DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag), ownerNames[resourceTable[r].owner]); + cliPrintf(format, DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag), ownerNames[resourceTable[r].owner]); if (resourceTable[r].maxIndex > 0) { cliPrintf(" %d", RESOURCE_INDEX(i)); } - cliPrint("\r\n"); - if (error) { - return false; + if (cleared) { + cliPrintf(". Cleared."); } + cliPrint("\r\n"); } } } - return true; } static void cliResource(char *cmdline) @@ -3430,7 +3411,7 @@ static void cliResource(char *cmdline) return; } else if (strncasecmp(cmdline, "list", len) == 0) { #ifdef MINIMAL_CLI - cliPrintf("Resources:\r\n"); + cliPrintf("IO\r\n"); #else cliPrintf("Currently active IO resource assignments:\r\n(reboot to update)\r\n"); cliRepeat('-', 20); @@ -3439,17 +3420,18 @@ static void cliResource(char *cmdline) const char* owner; owner = ownerNames[ioRecs[i].owner]; + cliPrintf("%c%02d: %s ", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner); if (ioRecs[i].index > 0) { - cliPrintf("%c%02d: %s %d\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, ioRecs[i].index); - } else { - cliPrintf("%c%02d: %s \r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner); - } + cliPrintf("%d", ioRecs[i].index); + } + cliPrintf("\r\n"); } + cliPrintf("\r\n\r\n"); #ifdef MINIMAL_CLI - cliPrintf("\r\n\r\nDMA:\r\n"); + cliPrintf("DMA:\r\n"); #else - cliPrintf("\r\n\r\nCurrently active DMA:\r\n"); + cliPrintf("Currently active DMA:\r\n"); cliRepeat('-', 20); #endif for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { @@ -3480,7 +3462,7 @@ static void cliResource(char *cmdline) pch = strtok_r(cmdline, " ", &saveptr); for (resourceIndex = 0; ; resourceIndex++) { if (resourceIndex >= ARRAYLEN(resourceTable)) { - cliPrint("Invalid resource\r\n"); + cliPrint("Invalid\r\n"); return; } @@ -3489,25 +3471,30 @@ static void cliResource(char *cmdline) } } - if (resourceTable[resourceIndex].maxIndex > 0) { - pch = strtok_r(NULL, " ", &saveptr); - index = atoi(pch); + pch = strtok_r(NULL, " ", &saveptr); + index = atoi(pch); - if (index <= 0 || index > resourceTable[resourceIndex].maxIndex) { - cliShowArgumentRangeError("index", 1, resourceTable[resourceIndex].maxIndex); + if (resourceTable[resourceIndex].maxIndex > 0 || index > 0) { + if (index <= 0 || index > MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)) { + cliShowArgumentRangeError("index", 1, MAX_RESOURCE_INDEX(resourceTable[resourceIndex].maxIndex)); return; } index -= 1; + + pch = strtok_r(NULL, " ", &saveptr); } - pch = strtok_r(NULL, " ", &saveptr); ioTag_t *tag = (ioTag_t*)(resourceTable[resourceIndex].ptr + index); uint8_t pin = 0; if (strlen(pch) > 0) { if (strcasecmp(pch, "NONE") == 0) { *tag = IO_TAG_NONE; - cliPrintf("Resource freed.\r\n"); +#ifdef MINIMAL_CLI + cliPrintf("Freed\r\n"); +#else + cliPrintf("Resource is freed\r\n"); +#endif return; } else { uint8_t port = (*pch) - 'A'; @@ -3521,14 +3508,15 @@ static void cliResource(char *cmdline) if (pin < 16) { ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin))); if (rec) { - if (!resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin))) { - return; - } - cliPrintf("\r\nResource set to %c%02d!\r\n", port + 'A', pin); - + resourceCheck(resourceIndex, index, DEFIO_TAG_MAKE(port, pin)); +#ifdef MINIMAL_CLI + cliPrintf(" %c%02d set\r\n", port + 'A', pin); +#else + cliPrintf("\r\nResource is set to %c%02d!\r\n", port + 'A', pin); +#endif *tag = DEFIO_TAG_MAKE(port, pin); } else { - cliPrintf("Resource invalid!\r\n"); + cliShowParseError(); } return; } From 02b778438f53348d35ff2350666d8c60d4d01f19 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 31 Jan 2017 00:40:15 +0100 Subject: [PATCH 31/55] Change Warning to Note --- src/main/fc/cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3b41f6b98..bb242e96c 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3375,7 +3375,7 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig) static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { - const char * format = "\r\n* WARNING * %c%d also used by %s"; + const char * format = "\r\n* NOTE * %c%d moved from %s"; for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) { if (*(resourceTable[r].ptr + i) == tag) { From 8f3ef2676fd8b5244aabe994c490a0b8bc17d942 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 21 Dec 2016 15:27:31 +0000 Subject: [PATCH 32/55] Tidied cms imu code --- src/main/cms/cms_menu_imu.c | 58 ++++++++++++++++++++----------------- 1 file changed, 32 insertions(+), 26 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 26cce4558..5899c69f8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -104,10 +104,11 @@ static long cmsx_rateProfileIndexOnChange(displayPort_t *displayPort, const void static long cmsx_PidRead(void) { + const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; for (uint8_t i = 0; i < 3; i++) { - tempPid[i][0] = masterConfig.profile[profileIndex].pidProfile.P8[i]; - tempPid[i][1] = masterConfig.profile[profileIndex].pidProfile.I8[i]; - tempPid[i][2] = masterConfig.profile[profileIndex].pidProfile.D8[i]; + tempPid[i][0] = pidProfile->P8[i]; + tempPid[i][1] = pidProfile->I8[i]; + tempPid[i][2] = pidProfile->D8[i]; } return 0; @@ -125,10 +126,11 @@ static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); + pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; for (uint8_t i = 0; i < 3; i++) { - masterConfig.profile[profileIndex].pidProfile.P8[i] = tempPid[i][0]; - masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1]; - masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2]; + pidProfile->P8[i] = tempPid[i][0]; + pidProfile->I8[i] = tempPid[i][1]; + pidProfile->D8[i] = tempPid[i][2]; } pidInitConfig(¤tProfile->pidProfile); @@ -233,12 +235,13 @@ static long cmsx_profileOtherOnEnter(void) { profileIndexString[1] = '0' + tmpProfileIndex; - cmsx_dtermSetpointWeight = masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight; - cmsx_setpointRelaxRatio = masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio; + const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + cmsx_dtermSetpointWeight = pidProfile->dtermSetpointWeight; + cmsx_setpointRelaxRatio = pidProfile->setpointRelaxRatio; - cmsx_angleStrength = masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL]; - cmsx_horizonStrength = masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL]; - cmsx_horizonTransition = masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL]; + cmsx_angleStrength = pidProfile->P8[PIDLEVEL]; + cmsx_horizonStrength = pidProfile->I8[PIDLEVEL]; + cmsx_horizonTransition = pidProfile->D8[PIDLEVEL]; return 0; } @@ -247,13 +250,14 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) { UNUSED(self); - masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight; - masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio; + pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile->dtermSetpointWeight = cmsx_dtermSetpointWeight; + pidProfile->setpointRelaxRatio = cmsx_setpointRelaxRatio; pidInitConfig(¤tProfile->pidProfile); - masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength; - masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength; - masterConfig.profile[profileIndex].pidProfile.D8[PIDLEVEL] = cmsx_horizonTransition; + pidProfile->P8[PIDLEVEL] = cmsx_angleStrength; + pidProfile->I8[PIDLEVEL] = cmsx_horizonStrength; + pidProfile->D8[PIDLEVEL] = cmsx_horizonTransition; return 0; } @@ -311,11 +315,12 @@ static uint16_t cmsx_yaw_p_limit; static long cmsx_FilterPerProfileRead(void) { - cmsx_dterm_lpf_hz = masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz; - cmsx_dterm_notch_hz = masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz; - cmsx_dterm_notch_cutoff = masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff; - cmsx_yaw_lpf_hz = masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz; - cmsx_yaw_p_limit = masterConfig.profile[profileIndex].pidProfile.yaw_p_limit; + const pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + cmsx_dterm_lpf_hz = pidProfile->dterm_lpf_hz; + cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; + cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; + cmsx_yaw_lpf_hz = pidProfile->yaw_lpf_hz; + cmsx_yaw_p_limit = pidProfile->yaw_p_limit; return 0; } @@ -324,11 +329,12 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) { UNUSED(self); - masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz; - masterConfig.profile[profileIndex].pidProfile.dterm_notch_hz = cmsx_dterm_notch_hz; - masterConfig.profile[profileIndex].pidProfile.dterm_notch_cutoff = cmsx_dterm_notch_cutoff; - masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz; - masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit; + pidProfile_t *pidProfile = &masterConfig.profile[profileIndex].pidProfile; + pidProfile->dterm_lpf_hz = cmsx_dterm_lpf_hz; + pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; + pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; + pidProfile->yaw_lpf_hz = cmsx_yaw_lpf_hz; + pidProfile->yaw_p_limit = cmsx_yaw_p_limit; return 0; } From 90e7bef9e4a286394dcc58f3e2941054ae5a65bd Mon Sep 17 00:00:00 2001 From: DieHertz Date: Tue, 27 Dec 2016 13:25:37 +0300 Subject: [PATCH 33/55] Added PWM inversion to motor config --- src/main/drivers/pwm_output.c | 14 ++++++++------ src/main/drivers/pwm_output.h | 2 +- src/main/drivers/pwm_output_stm32f3xx.c | 8 ++++---- src/main/drivers/pwm_output_stm32f4xx.c | 8 ++++---- src/main/drivers/pwm_output_stm32f7xx.c | 12 ++++++++---- src/main/fc/cli.c | 1 + src/main/fc/fc_msp.c | 7 ++++++- src/main/io/motors.h | 3 ++- 8 files changed, 34 insertions(+), 21 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 8f99c16ad..eb7181801 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -83,7 +83,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 #endif } -static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion) { #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); @@ -91,7 +91,8 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard #endif configTimeBase(timerHardware->tim, period, mhz); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value, + inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); #if defined(USE_HAL_DRIVER) HAL_TIM_PWM_Start(Handle, timerHardware->channel); @@ -259,7 +260,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot #ifdef USE_DSHOT if (isDigital) { - pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol); + pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, + motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motors[motorIndex].enabled = true; continue; } @@ -274,9 +276,9 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot if (useUnsyncedPwm) { const uint32_t hz = timerMhzCounter * 1000000; - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion); } else { - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion); } bool timerAlreadyUsed = false; @@ -348,7 +350,7 @@ void servoInit(const servoConfig_t *servoConfig) break; } - pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse); + pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); servos[servoIndex].enabled = true; } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7102e7e35..97c5f5e66 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -119,7 +119,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDigital(uint8_t index, uint16_t value); -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType); +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); #endif diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 5ae299dcf..9e29cedaa 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -105,7 +105,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; @@ -139,14 +139,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (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; + TIM_OCInitStructure.TIM_OCNPolarity = (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 = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 03156953a..95d53b3d5 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; @@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (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_High : TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } 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 = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index b89849325..848cb88ea 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) UNUSED(motorCount); } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; @@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; - TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH; - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; + TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + } else { + TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; + } TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.Pulse = 0; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index bb242e96c..87ca6b2e1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -524,6 +524,7 @@ const clivalue_t valueTable[] = { { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } }, + { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE, &motorConfig()->motorPwmInversion, .config.lookup = { TABLE_OFF_ON } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 404a89546..8e74b2893 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1144,6 +1144,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); //!!TODO gyro_isr_update to be added pending decision //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); + sbufWriteU8(dst, motorConfig()->motorPwmInversion); break; case MSP_FILTER_CONFIG : @@ -1480,7 +1481,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif motorConfig()->motorPwmRate = sbufReadU16(src); - if (dataSize > 7) { + if (sbufBytesRemaining(src) >= 2) { motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } if (sbufBytesRemaining(src)) { @@ -1491,6 +1492,10 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gyroConfig()->gyro_isr_update = sbufReadU8(src); }*/ validateAndFixGyroConfig(); + + if (sbufBytesRemaining(src)) { + motorConfig()->motorPwmInversion = sbufReadU8(src); + } break; case MSP_SET_FILTER_CONFIG: diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 8486d507d..82517b2e9 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -24,8 +24,9 @@ typedef struct motorConfig_s { uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint16_t motorPwmRate; // The update rate of motor outputs uint8_t motorPwmProtocol; // Pwm Protocol + uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation uint8_t useUnsyncedPwm; float digitalIdleOffsetPercent; ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; From 135acdddaadb0211259128490740d1757b04c686 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 24 Jan 2017 22:38:36 +0000 Subject: [PATCH 34/55] Reviewed and revised compiler speed optimisations --- Makefile | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/Makefile b/Makefile index 56c02912f..012a03b23 100644 --- a/Makefile +++ b/Makefile @@ -708,26 +708,20 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/serial.c \ drivers/serial_uart.c \ drivers/sound_beeper.c \ - drivers/stack_check.c \ drivers/system.c \ drivers/timer.c \ + fc/fc_core.c \ fc/fc_tasks.c \ fc/fc_rc.c \ fc/rc_controls.c \ - fc/rc_curves.c \ fc/runtime_config.c \ - flight/altitudehold.c \ - flight/failsafe.c \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ flight/servos.c \ - io/beeper.c \ io/serial.c \ - io/statusindicator.c \ rx/ibus.c \ rx/jetiexbus.c \ - rx/msp.c \ rx/nrf24_cx10.c \ rx/nrf24_inav.c \ rx/nrf24_h8_3d.c \ @@ -748,25 +742,12 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ sensors/gyro.c \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ drivers/serial_softserial.c \ io/dashboard.c \ io/displayport_max7456.c \ - io/displayport_msp.c \ - io/displayport_oled.c \ - io/ledstrip.c \ io/osd.c \ - telemetry/telemetry.c \ - telemetry/crsf.c \ - telemetry/frsky.c \ - telemetry/hott.c \ - telemetry/smartport.c \ - telemetry/ltm.c \ - telemetry/mavlink.c \ - telemetry/esc_telemetry.c \ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/serial_escserial.c \ @@ -952,7 +933,7 @@ LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT) else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) OPTIMISE_DEFAULT := -O2 -OPTIMISE_SPEED := -Ofast +OPTIMISE_SPEED := -O3 OPTIMISE_SIZE := -Os LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) From 0aa83ad4f80159380c378a82df44509de0ee3839 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 17:08:51 +0000 Subject: [PATCH 35/55] Updated version, MSP version and EEPROM_CONF_VERSION for 3.2 --- src/main/build/version.h | 4 ++-- src/main/msp/msp_protocol.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index 3df77b8cf..4770ad98e 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -17,8 +17,8 @@ #define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 63d2b70bd..d2c368f28 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 31 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MINOR 32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) #define API_VERSION_LENGTH 2 From b8b790ba1fef6e89efcc4d1905a627179ce63586 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:16:05 +0000 Subject: [PATCH 36/55] Reorder accgyro_mpu.c functions for clarity and to avoid forward declarations --- src/main/drivers/accgyro_mpu.c | 227 ++++++++++++++++----------------- 1 file changed, 109 insertions(+), 118 deletions(-) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index b452afc77..93405ba68 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -50,15 +50,6 @@ mpuResetFuncPtr mpuReset; -static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data); -static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data); - -static void mpu6050FindRevision(gyroDev_t *gyro); - -#ifdef USE_SPI -static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro); -#endif - #ifndef MPU_I2C_INSTANCE #define MPU_I2C_INSTANCE I2C_DEVICE #endif @@ -71,110 +62,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro); #define MPU_INQUIRY_MASK 0x7E -mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) -{ - bool ack; - uint8_t sig; - uint8_t inquiryResult; - - // MPU datasheet specifies 30ms. - delay(35); - -#ifndef USE_I2C - ack = false; - sig = 0; -#else - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); -#endif - if (ack) { - gyro->mpuConfiguration.read = mpuReadRegisterI2C; - gyro->mpuConfiguration.write = mpuWriteRegisterI2C; - } else { -#ifdef USE_SPI - bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); - UNUSED(detectedSpiSensor); -#endif - - return &gyro->mpuDetectionResult; - } - - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - - // If an MPU3050 is connected sig will contain 0. - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); - inquiryResult &= MPU_INQUIRY_MASK; - if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_3050; - gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; - return &gyro->mpuDetectionResult; - } - - sig &= MPU_INQUIRY_MASK; - - if (sig == MPUx0x0_WHO_AM_I_CONST) { - - gyro->mpuDetectionResult.sensor = MPU_60x0; - - mpu6050FindRevision(gyro); - } else if (sig == MPU6500_WHO_AM_I_CONST) { - gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; - } - - return &gyro->mpuDetectionResult; -} - -#ifdef USE_SPI -static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) -{ -#ifdef USE_GYRO_SPI_MPU6500 - uint8_t mpu6500Sensor = mpu6500SpiDetect(); - if (mpu6500Sensor != MPU_NONE) { - gyro->mpuDetectionResult.sensor = mpu6500Sensor; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6500ReadRegister; - gyro->mpuConfiguration.write = mpu6500WriteRegister; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_ICM20689 - if (icm20689SpiDetect()) { - gyro->mpuDetectionResult.sensor = ICM_20689_SPI; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = icm20689ReadRegister; - gyro->mpuConfiguration.write = icm20689WriteRegister; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_MPU6000 - if (mpu6000SpiDetect()) { - gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6000ReadRegister; - gyro->mpuConfiguration.write = mpu6000WriteRegister; - return true; - } -#endif - -#ifdef USE_GYRO_SPI_MPU9250 - if (mpu9250SpiDetect()) { - gyro->mpuDetectionResult.sensor = MPU_9250_SPI; - gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu9250ReadRegister; - gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister; - gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister; - gyro->mpuConfiguration.write = mpu9250WriteRegister; - gyro->mpuConfiguration.reset = mpu9250ResetGyro; - return true; - } -#endif - - UNUSED(gyro); - return false; -} -#endif - static void mpu6050FindRevision(gyroDev_t *gyro) { bool ack; @@ -324,11 +211,6 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } -void mpuGyroInit(gyroDev_t *gyro) -{ - mpuIntExtiInit(gyro); -} - bool mpuCheckDataReady(gyroDev_t* gyro) { bool ret; @@ -340,3 +222,112 @@ bool mpuCheckDataReady(gyroDev_t* gyro) } return ret; } + +#ifdef USE_SPI +static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_SPI_MPU6000 + if (mpu6000SpiDetect()) { + gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.read = mpu6000ReadRegister; + gyro->mpuConfiguration.write = mpu6000WriteRegister; + return true; + } +#endif + +#ifdef USE_GYRO_SPI_MPU6500 + uint8_t mpu6500Sensor = mpu6500SpiDetect(); + if (mpu6500Sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = mpu6500Sensor; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.read = mpu6500ReadRegister; + gyro->mpuConfiguration.write = mpu6500WriteRegister; + return true; + } +#endif + +#ifdef USE_GYRO_SPI_MPU9250 + if (mpu9250SpiDetect()) { + gyro->mpuDetectionResult.sensor = MPU_9250_SPI; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.read = mpu9250ReadRegister; + gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister; + gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister; + gyro->mpuConfiguration.write = mpu9250WriteRegister; + gyro->mpuConfiguration.reset = mpu9250ResetGyro; + return true; + } +#endif + +#ifdef USE_GYRO_SPI_ICM20689 + if (icm20689SpiDetect()) { + gyro->mpuDetectionResult.sensor = ICM_20689_SPI; + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + gyro->mpuConfiguration.read = icm20689ReadRegister; + gyro->mpuConfiguration.write = icm20689WriteRegister; + return true; + } +#endif + + UNUSED(gyro); + return false; +} +#endif + +mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) +{ + bool ack; + uint8_t sig; + uint8_t inquiryResult; + + // MPU datasheet specifies 30ms. + delay(35); + +#ifndef USE_I2C + ack = false; + sig = 0; +#else + ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); +#endif + if (ack) { + gyro->mpuConfiguration.read = mpuReadRegisterI2C; + gyro->mpuConfiguration.write = mpuWriteRegisterI2C; + } else { +#ifdef USE_SPI + bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); + UNUSED(detectedSpiSensor); +#endif + + return &gyro->mpuDetectionResult; + } + + gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; + + // If an MPU3050 is connected sig will contain 0. + ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); + inquiryResult &= MPU_INQUIRY_MASK; + if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_3050; + gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; + return &gyro->mpuDetectionResult; + } + + sig &= MPU_INQUIRY_MASK; + + if (sig == MPUx0x0_WHO_AM_I_CONST) { + + gyro->mpuDetectionResult.sensor = MPU_60x0; + + mpu6050FindRevision(gyro); + } else if (sig == MPU6500_WHO_AM_I_CONST) { + gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; + } + + return &gyro->mpuDetectionResult; +} + +void mpuGyroInit(gyroDev_t *gyro) +{ + mpuIntExtiInit(gyro); +} From 6dc16ce42c8013fe03632055b77124da54609796 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:49:32 +0000 Subject: [PATCH 37/55] Changed back to using -Ofast optimisation --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 012a03b23..5e9cbb630 100644 --- a/Makefile +++ b/Makefile @@ -933,7 +933,7 @@ LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_DEFAULT) else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) OPTIMISE_DEFAULT := -O2 -OPTIMISE_SPEED := -O3 +OPTIMISE_SPEED := -Ofast OPTIMISE_SIZE := -Os LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) From 82405fd50cae4e1835f15db0c2f1ce7704dbcb6b Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 17 Jan 2017 01:04:38 +0100 Subject: [PATCH 38/55] some fixes for vtx_common --- src/main/drivers/vtx_common.c | 10 ++++-- src/main/drivers/vtx_common.h | 7 +++-- src/main/io/vtx_tramp.c | 57 +++++++++++++++++------------------ 3 files changed, 40 insertions(+), 34 deletions(-) diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 7e617b03a..e43505d26 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -53,10 +53,10 @@ void vtxCommonProcess(uint32_t currentTimeUs) vtxDevType_e vtxCommonGetDeviceType(void) { - if (!vtxDevice) + if (!vtxDevice || !vtxDevice->vTable->getDeviceType) return VTXDEV_UNKNOWN; - return vtxDevice->devtype; + return vtxDevice->vTable->getDeviceType(); } // band and chan are 1 origin @@ -65,6 +65,9 @@ void vtxCommonSetBandChan(uint8_t band, uint8_t chan) if (!vtxDevice) return; + if ((band > vtxDevice->numBand)|| (chan > vtxDevice->numChan)) + return; + if (vtxDevice->vTable->setBandChan) vtxDevice->vTable->setBandChan(band, chan); } @@ -75,6 +78,9 @@ void vtxCommonSetPowerByIndex(uint8_t index) if (!vtxDevice) return; + if (index > vtxDevice->numPower) + return; + if (vtxDevice->vTable->setPowerByIndex) vtxDevice->vTable->setPowerByIndex(index); } diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index c7c521c66..e0899cc85 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -18,9 +18,12 @@ /* Created by jflyper */ typedef enum { - VTXDEV_UNKNOWN = 0, + VTXDEV_UNSUPPORTED = 0, // reserved for MSP + // 1 reserved + // 2 reserved VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, + VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; struct vtxVTable_s; @@ -28,8 +31,6 @@ struct vtxVTable_s; typedef struct vtxDevice_s { const struct vtxVTable_s *vTable; - vtxDevType_e devtype; // 3.1 only; eventually goes away - uint8_t numBand; uint8_t numChan; uint8_t numPower; diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 749a0d40c..34da6e97f 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -46,14 +46,13 @@ static const uint16_t trampPowerTable[] = { }; static const char * const trampPowerNames[] = { - "25 ", "100", "200", "400", "600" + "---", "25 ", "100", "200", "400", "600" }; #endif #if defined(VTX_COMMON) -static vtxVTable_t trampVTable; // Forward static vtxDevice_t vtxTramp = { - .vTable = &trampVTable, + .vTable = NULL, .numBand = 5, .numChan = 8, .numPower = sizeof(trampPowerTable), @@ -309,26 +308,6 @@ void trampQueryS(void) trampQuery('s'); } -bool trampInit() -{ - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); - - if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); - } - - if (!trampSerialPort) { - return false; - } - -#if defined(VTX_COMMON) - vtxTramp.vTable = &trampVTable; - vtxCommonRegisterDevice(&vtxTramp); -#endif - - return true; -} - void vtxTrampProcess(uint32_t currentTimeUs) { static uint32_t lastQueryTimeUs = 0; @@ -472,9 +451,9 @@ static OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames, NULL } static OSD_UINT16_t trampCmsEntFreqRef = { &trampCmsFreqRef, 5600, 5900, 0 }; -static uint8_t trampCmsPower = 0; +static uint8_t trampCmsPower = 1; -static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 4, trampPowerNames, NULL }; +static OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames, NULL }; static void trampCmsUpdateFreqRef(void) { @@ -539,7 +518,7 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(self); trampSetBandChan(trampCmsBand, trampCmsChan); - trampSetRFPower(trampPowerTable[trampCmsPower]); + trampSetRFPower(trampPowerTable[trampCmsPower-1]); // If it fails, the user should retry later trampCommitChanges(); @@ -559,7 +538,7 @@ static void trampCmsInitSettings() if (trampCurConfigPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { if (trampCurConfigPower <= trampPowerTable[i]) { - trampCmsPower = i; + trampCmsPower = i + 1; break; } } @@ -640,7 +619,7 @@ void vtxTrampSetBandChan(uint8_t band, uint8_t chan) void vtxTrampSetPowerByIndex(uint8_t index) { if (index) { - trampSetRFPower(trampPowerTable[index]); + trampSetRFPower(trampPowerTable[index - 1]); trampCommitChanges(); } } @@ -668,7 +647,7 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) if (trampCurConfigPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { if (trampCurConfigPower <= trampPowerTable[i]) { - *pIndex = i; + *pIndex = i + 1; break; } } @@ -700,4 +679,24 @@ static vtxVTable_t trampVTable = { #endif +bool trampInit() +{ + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); + + if (portConfig) { + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + } + + if (!trampSerialPort) { + return false; + } + +#if defined(VTX_COMMON) + vtxTramp.vTable = &trampVTable; + vtxCommonRegisterDevice(&vtxTramp); +#endif + + return true; +} + #endif // VTX_TRAMP From 50ddce722787c13505b5e784d3225165b452faf3 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 17 Jan 2017 01:06:20 +0100 Subject: [PATCH 39/55] added MSP_VTX_CONFIG & MSP_SET_VTX_CONFIG --- src/main/fc/fc_msp.c | 75 +++++++++++++++++++++++++++++++++++++++----- 1 file changed, 67 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8e74b2893..6b3b86a87 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -47,6 +47,7 @@ #include "drivers/vtx_soft_spi_rtc6705.h" #include "drivers/pwm_output.h" #include "drivers/serial_escserial.h" +#include "drivers/vtx_common.h" #include "fc/config.h" #include "fc/fc_core.h" @@ -1188,6 +1189,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } break; +#if defined(VTX_COMMON) + case MSP_VTX_CONFIG: + { + uint8_t deviceType = vtxCommonGetDeviceType(); + if (deviceType != VTXDEV_UNKNOWN) { + + uint8_t band=0, channel=0; + vtxCommonGetBandChan(&band,&channel); + + uint8_t powerIdx=0; // debug + vtxCommonGetPowerIndex(&powerIdx); + + uint8_t pitmode=0; + vtxCommonGetPitmode(&pitmode); + + sbufWriteU8(dst, deviceType); + sbufWriteU8(dst, band); + sbufWriteU8(dst, channel); + sbufWriteU8(dst, powerIdx); + sbufWriteU8(dst, pitmode); + // future extensions here... + } + else { + sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected + } + } +#endif + break; + default: return false; } @@ -1639,15 +1669,44 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; #endif -#ifdef USE_RTC6705 +#if defined(USE_RTC6705) || defined(VTX_COMMON) case MSP_SET_VTX_CONFIG: - ; - uint16_t tmp = sbufReadU16(src); - if (tmp < 40) - masterConfig.vtx_channel = tmp; - if (current_vtx_channel != masterConfig.vtx_channel) { - current_vtx_channel = masterConfig.vtx_channel; - rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); + { + uint16_t tmp = sbufReadU16(src); +#if defined(USE_RTC6705) + if (tmp < 40) + masterConfig.vtx_channel = tmp; + if (current_vtx_channel != masterConfig.vtx_channel) { + current_vtx_channel = masterConfig.vtx_channel; + rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); + } +#else + if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) { + + uint8_t band = (tmp / 8) + 1; + uint8_t channel = (tmp % 8) + 1; + + uint8_t current_band=0, current_channel=0; + vtxCommonGetBandChan(¤t_band,¤t_channel); + if ((current_band != band) || (current_channel != channel)) + vtxCommonSetBandChan(band,channel); + + if (sbufBytesRemaining(src) < 2) + break; + + uint8_t power = sbufReadU8(src); + uint8_t current_power = 0; + vtxCommonGetPowerIndex(¤t_power); + if (current_power != power) + vtxCommonSetPowerByIndex(power); + + uint8_t pitmode = sbufReadU8(src); + uint8_t current_pitmode = 0; + vtxCommonGetPitmode(¤t_pitmode); + if (current_pitmode != pitmode) + vtxCommonSetPitmode(pitmode); + } +#endif } break; #endif From 0307bf6f87d33437bcba9d1d9d5cef356fcb702b Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 26 Jan 2017 08:56:22 +1300 Subject: [PATCH 40/55] Rebase of #1917: Update SDK to 6.2.1 2016q4 (thanks to @TheAngularity). --- .travis.yml | 2 +- make/tools.mk | 10 +++++----- src/main/build/atomic.h | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 507232492..df472a95e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -82,7 +82,7 @@ install: - make arm_sdk_install before_script: - - tools/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gcc --version + - tools/gcc-arm-none-eabi-6_2-2016q4/bin/arm-none-eabi-gcc --version - clang --version - clang++ --version diff --git a/make/tools.mk b/make/tools.mk index 810716a57..4ad98115d 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -14,16 +14,16 @@ ############################## # Set up ARM (STM32) SDK -ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q3 +ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-6_2-2016q4 # Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion) -GCC_REQUIRED_VERSION ?= 5.4.1 +GCC_REQUIRED_VERSION ?= 6.2.1 ## arm_sdk_install : Install Arm SDK .PHONY: arm_sdk_install -ARM_SDK_URL_BASE := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926 +ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2016q4/gcc-arm-none-eabi-6_2-2016q4-20161216 -# source: https://launchpad.net/gcc-arm-embedded/5.0/ +# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads ifdef LINUX ARM_SDK_URL := $(ARM_SDK_URL_BASE)-linux.tar.bz2 endif @@ -33,7 +33,7 @@ ifdef MACOSX endif ifdef WINDOWS - ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip + ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32-zip.zip endif ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 4603091ed..959c6e9d8 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -85,7 +85,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) // ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full barrier // this macro can be used only ONCE PER LINE, but multiple uses per block are fine -#if (__GNUC__ > 5) +#if (__GNUC__ > 6) #warning "Please verify that ATOMIC_BARRIER works as intended" // increment version number is BARRIER works // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead From 44f4e479eb9de56c92f0e58da65d848d787cdc27 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 20 Dec 2016 22:42:58 +0000 Subject: [PATCH 41/55] Parameter groups EEPROM migration --- Makefile | 1 + src/main/config/config_eeprom.c | 475 +++++++++++++--------------- src/main/config/config_eeprom.h | 5 +- src/main/config/config_profile.h | 1 - src/main/config/config_reset.h | 40 +++ src/main/config/config_streamer.c | 177 +++++++++++ src/main/config/config_streamer.h | 45 +++ src/main/fc/config.c | 31 +- src/main/fc/config.h | 5 + src/main/fc/fc_msp.c | 1 - src/main/target/link/stm32_flash.ld | 13 + 11 files changed, 539 insertions(+), 255 deletions(-) create mode 100644 src/main/config/config_reset.h create mode 100644 src/main/config/config_streamer.c create mode 100644 src/main/config/config_streamer.h diff --git a/Makefile b/Makefile index 5e9cbb630..e5bc1e4ad 100644 --- a/Makefile +++ b/Makefile @@ -561,6 +561,7 @@ COMMON_SRC = \ config/config_eeprom.c \ config/feature.c \ config/parameter_group.c \ + config/config_streamer.c \ drivers/adc.c \ drivers/buf_writer.c \ drivers/bus_i2c_soft.c \ diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 5c32f1818..18cbfa8e9 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -18,294 +18,271 @@ #include #include #include +#include #include "platform.h" -#include "drivers/system.h" - -#include "config/config_master.h" - #include "build/build_config.h" +#include "common/maths.h" + #include "config/config_eeprom.h" +#include "config/config_streamer.h" +#include "config/config_master.h" +#include "config/parameter_group.h" -#if !defined(FLASH_SIZE) -#error "Flash size not defined for target. (specify in KB)" -#endif +#include "drivers/system.h" +#include "fc/config.h" -#ifndef FLASH_PAGE_SIZE - #ifdef STM32F303xC - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; - #ifdef STM32F10X_MD - #define FLASH_PAGE_SIZE ((uint16_t)0x400) - #endif +typedef enum { + CR_CLASSICATION_SYSTEM = 0, + CR_CLASSICATION_PROFILE1 = 1, + CR_CLASSICATION_PROFILE2 = 2, + CR_CLASSICATION_PROFILE3 = 3, + CR_CLASSICATION_PROFILE_LAST = CR_CLASSICATION_PROFILE3, +} configRecordFlags_e; - #ifdef STM32F10X_HD - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif +#define CR_CLASSIFICATION_MASK (0x3) - #if defined(STM32F745xx) - #define FLASH_PAGE_SIZE ((uint32_t)0x40000) - #endif +// Header for the saved copy. +typedef struct { + uint8_t format; +} PG_PACKED configHeader_t; - #if defined(STM32F746xx) - #define FLASH_PAGE_SIZE ((uint32_t)0x40000) - #endif +// Header for each stored PG. +typedef struct { + // split up. + uint16_t size; + pgn_t pgn; + uint8_t version; - #if defined(STM32F722xx) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif + // lower 2 bits used to indicate system or profile number, see CR_CLASSIFICATION_MASK + uint8_t flags; - #if defined(STM32F40_41xxx) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif + uint8_t pg[]; +} PG_PACKED configRecord_t; - #if defined (STM32F411xE) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif - -#endif - -#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT) - #ifdef STM32F10X_MD - #define FLASH_PAGE_COUNT 128 - #endif - - #ifdef STM32F10X_HD - #define FLASH_PAGE_COUNT 128 - #endif -#endif - -#if defined(FLASH_SIZE) -#if defined(STM32F40_41xxx) -#define FLASH_PAGE_COUNT 4 -#elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 3 -#elif defined (STM32F722xx) -#define FLASH_PAGE_COUNT 3 -#elif defined (STM32F745xx) -#define FLASH_PAGE_COUNT 4 -#elif defined (STM32F746xx) -#define FLASH_PAGE_COUNT 4 -#else -#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE) -#endif -#endif - -#if !defined(FLASH_PAGE_SIZE) -#error "Flash page size not defined for target." -#endif - -#if !defined(FLASH_PAGE_COUNT) -#error "Flash page count not defined for target." -#endif - -#if FLASH_SIZE <= 128 -#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 -#else -#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000 -#endif - -// use the last flash pages for storage -#ifdef CUSTOM_FLASH_MEMORY_ADDRESS -size_t custom_flash_memory_address = 0; -#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) -#else -// use the last flash pages for storage -#ifndef CONFIG_START_FLASH_ADDRESS -#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) -#endif -#endif +// Footer for the saved copy. +typedef struct { + uint16_t terminator; +} PG_PACKED configFooter_t; +// checksum is appended just after footer. It is not included in footer to make checksum calculation consistent +// Used to check the compiler packing at build time. +typedef struct { + uint8_t byte; + uint32_t word; +} PG_PACKED packingTest_t; void initEEPROM(void) { + // Verify that this architecture packs as expected. + BUILD_BUG_ON(offsetof(packingTest_t, byte) != 0); + BUILD_BUG_ON(offsetof(packingTest_t, word) != 1); + BUILD_BUG_ON(sizeof(packingTest_t) != 5); + + BUILD_BUG_ON(sizeof(configHeader_t) != 1); + BUILD_BUG_ON(sizeof(configFooter_t) != 2); + BUILD_BUG_ON(sizeof(configRecord_t) != 6); } -static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) +static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length) { - uint8_t checksum = 0; - const uint8_t *byteOffset; + const uint8_t *p = (const uint8_t *)data; + const uint8_t *pend = p + length; - for (byteOffset = data; byteOffset < (data + length); byteOffset++) - checksum ^= *byteOffset; - return checksum; + for (; p != pend; p++) { + chk ^= *p; + } + return chk; +} + +// Scan the EEPROM config. Returns true if the config is valid. +static bool scanEEPROM(void) +{ + uint8_t chk = 0; + const uint8_t *p = &__config_start; + const configHeader_t *header = (const configHeader_t *)p; + + if (header->format != EEPROM_CONF_VERSION) { + return false; + } + chk = updateChecksum(chk, header, sizeof(*header)); + p += sizeof(*header); + // include the transitional masterConfig record + chk = updateChecksum(chk, p, sizeof(masterConfig)); + p += sizeof(masterConfig); + + for (;;) { + const configRecord_t *record = (const configRecord_t *)p; + + if (record->size == 0) { + // Found the end. Stop scanning. + break; + } + if (p + record->size >= &__config_end + || record->size < sizeof(*record)) { + // Too big or too small. + return false; + } + + chk = updateChecksum(chk, p, record->size); + + p += record->size; + } + + const configFooter_t *footer = (const configFooter_t *)p; + chk = updateChecksum(chk, footer, sizeof(*footer)); + p += sizeof(*footer); + chk = ~chk; + return chk == *p; +} + +// find config record for reg + classification (profile info) in EEPROM +// return NULL when record is not found +// this function assumes that EEPROM content is valid +static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFlags_e classification) +{ + const uint8_t *p = &__config_start; + p += sizeof(configHeader_t); // skip header + p += sizeof(master_t); // skip the transitional master_t record + while (true) { + const configRecord_t *record = (const configRecord_t *)p; + if (record->size == 0 + || p + record->size >= &__config_end + || record->size < sizeof(*record)) + break; + if (pgN(reg) == record->pgn + && (record->flags & CR_CLASSIFICATION_MASK) == classification) + return record; + p += record->size; + } + // record not found + return NULL; +} + +// Initialize all PG records from EEPROM. +// This functions processes all PGs sequentially, scanning EEPROM for each one. This is suboptimal, +// but each PG is loaded/initialized exactly once and in defined order. +bool loadEEPROM(void) +{ + // read in the transitional masterConfig record + const uint8_t *p = &__config_start; + p += sizeof(configHeader_t); // skip header + masterConfig = *(master_t*)p; + + PG_FOREACH(reg) { + configRecordFlags_e cls_start, cls_end; + if (pgIsSystem(reg)) { + cls_start = CR_CLASSICATION_SYSTEM; + cls_end = CR_CLASSICATION_SYSTEM; + } else { + cls_start = CR_CLASSICATION_PROFILE1; + cls_end = CR_CLASSICATION_PROFILE_LAST; + } + for (configRecordFlags_e cls = cls_start; cls <= cls_end; cls++) { + int profileIndex = cls - cls_start; + const configRecord_t *rec = findEEPROM(reg, cls); + if (rec) { + // config from EEPROM is available, use it to initialize PG. pgLoad will handle version mismatch + pgLoad(reg, profileIndex, rec->pg, rec->size - offsetof(configRecord_t, pg), rec->version); + } else { + pgReset(reg, profileIndex); + } + } + } + return true; } bool isEEPROMContentValid(void) { - const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; - uint8_t checksum = 0; - - // check version number - if (EEPROM_CONF_VERSION != temp->version) - return false; - - // check size and magic numbers - if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) - return false; - - if (strncasecmp(temp->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) - return false; - - // verify integrity of temporary copy - checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); - if (checksum != 0) - return false; - - // looks good, let's roll! - return true; + return scanEEPROM(); } -#if defined(STM32F7) - -// FIXME: HAL for now this will only work for F4/F7 as flash layout is different -void writeEEPROM(void) +static bool writeSettingsToEEPROM(void) { - // Generate compile time error if the config does not fit in the reserved area of flash. - BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); + config_streamer_t streamer; + config_streamer_init(&streamer); - HAL_StatusTypeDef status; - uint32_t wordOffset; - int8_t attemptsRemaining = 3; + config_streamer_start(&streamer, (uintptr_t)&__config_start, &__config_end - &__config_start); + uint8_t chk = 0; - suspendRxSignal(); + configHeader_t header = { + .format = EEPROM_CONF_VERSION, + }; - // prepare checksum/version constants - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.size = sizeof(master_t); - masterConfig.magic_be = 0xBE; - masterConfig.magic_ef = 0xEF; - masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); + config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)); + chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header)); + // write the transitional masterConfig record + config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig)); + chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig)); + PG_FOREACH(reg) { + const uint16_t regSize = pgSize(reg); + configRecord_t record = { + .size = sizeof(configRecord_t) + regSize, + .pgn = pgN(reg), + .version = pgVersion(reg), + .flags = 0 + }; - // write it - /* Unlock the Flash to enable the flash control register access *************/ - HAL_FLASH_Unlock(); - while (attemptsRemaining--) - { - /* Fill EraseInit structure*/ - FLASH_EraseInitTypeDef EraseInitStruct = {0}; - EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; - EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V - EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1); - EraseInitStruct.NbSectors = 1; - uint32_t SECTORError; - status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); - if (status != HAL_OK) - { - continue; - } - else - { - for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) - { - status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, CONFIG_START_FLASH_ADDRESS + wordOffset, *(uint32_t *) ((char *) &masterConfig + wordOffset)); - if(status != HAL_OK) - { - break; - } + if (pgIsSystem(reg)) { + // write the only instance + record.flags |= CR_CLASSICATION_SYSTEM; + config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); + chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record)); + config_streamer_write(&streamer, reg->address, regSize); + chk = updateChecksum(chk, reg->address, regSize); + } else { + // write one instance for each profile + for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) { + record.flags = 0; + + record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK); + config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)); + chk = updateChecksum(chk, (uint8_t *)&record, sizeof(record)); + const uint8_t *address = reg->address + (regSize * profileIndex); + config_streamer_write(&streamer, address, regSize); + chk = updateChecksum(chk, address, regSize); } } - if (status == HAL_OK) { - break; + } + + configFooter_t footer = { + .terminator = 0, + }; + + config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)); + chk = updateChecksum(chk, (uint8_t *)&footer, sizeof(footer)); + + // append checksum now + chk = ~chk; + config_streamer_write(&streamer, &chk, sizeof(chk)); + + config_streamer_flush(&streamer); + + bool success = config_streamer_finish(&streamer) == 0; + + return success; +} + +void writeConfigToEEPROM(void) +{ + bool success = false; + // write it + for (int attempt = 0; attempt < 3 && !success; attempt++) { + if (writeSettingsToEEPROM()) { + success = true; } } - HAL_FLASH_Lock(); + + if (success && isEEPROMContentValid()) { + return; + } // Flash write failed - just die now - if (status != HAL_OK || !isEEPROMContentValid()) { - failureMode(FAILURE_FLASH_WRITE_FAILED); - } - - resumeRxSignal(); -} -#else -void writeEEPROM(void) -{ - // Generate compile time error if the config does not fit in the reserved area of flash. - BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); - - FLASH_Status status = 0; - uint32_t wordOffset; - int8_t attemptsRemaining = 3; - - suspendRxSignal(); - - // prepare checksum/version constants - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.size = sizeof(master_t); - masterConfig.magic_be = 0xBE; - masterConfig.magic_ef = 0xEF; - masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); - - // write it - FLASH_Unlock(); - while (attemptsRemaining--) { -#if defined(STM32F4) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); -#elif defined(STM32F303) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); -#elif defined(STM32F10X) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); -#endif - for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { - if (wordOffset % FLASH_PAGE_SIZE == 0) { -#if defined(STM32F40_41xxx) - status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 -#elif defined (STM32F411xE) - status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 -#else - status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); -#endif - if (status != FLASH_COMPLETE) { - break; - } - } - - status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, - *(uint32_t *) ((char *) &masterConfig + wordOffset)); - if (status != FLASH_COMPLETE) { - break; - } - } - if (status == FLASH_COMPLETE) { - break; - } - } - FLASH_Lock(); - - // Flash write failed - just die now - if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { - failureMode(FAILURE_FLASH_WRITE_FAILED); - } - - resumeRxSignal(); -} -#endif - -void readEEPROM(void) -{ - // Sanity check - if (!isEEPROMContentValid()) - failureMode(FAILURE_INVALID_EEPROM_CONTENTS); - - suspendRxSignal(); - - // Read flash - memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); - - if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check - masterConfig.current_profile_index = 0; - - setProfile(masterConfig.current_profile_index); - - validateAndFixConfig(); - activateConfig(); - - resumeRxSignal(); + failureMode(FAILURE_FLASH_WRITE_FAILED); } diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 645689ffd..a6aafc3ca 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -19,7 +19,6 @@ #define EEPROM_CONF_VERSION 154 -void initEEPROM(void); -void writeEEPROM(); -void readEEPROM(void); bool isEEPROMContentValid(void); +bool loadEEPROM(void); +void writeConfigToEEPROM(void); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index a0ffa5390..d1f2c513f 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -17,7 +17,6 @@ #pragma once -#include "common/axis.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "flight/pid.h" diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h new file mode 100644 index 000000000..7ca0eda77 --- /dev/null +++ b/src/main/config/config_reset.h @@ -0,0 +1,40 @@ +/* + * 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 + +#ifndef __UNIQL +# define __UNIQL_CONCAT2(x,y) x ## y +# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y) +# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__) +#endif + +// overwrite _name with data passed as arguments. This version forces GCC to really copy data +// It is not possible to use multiple RESET_CONFIGs on single line (__UNIQL limitation) +#define RESET_CONFIG(_type, _name, ...) \ + static const _type __UNIQL(_reset_template_) = { \ + __VA_ARGS__ \ + }; \ + memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name))); \ + /**/ + +// overwrite _name with data passed as arguments. GCC is alloved to set structure field-by-field +#define RESET_CONFIG_2(_type, _name, ...) \ + *(_name) = (_type) { \ + __VA_ARGS__ \ + }; \ + /**/ diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c new file mode 100644 index 000000000..cd48ea9f7 --- /dev/null +++ b/src/main/config/config_streamer.c @@ -0,0 +1,177 @@ +/* + * 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 "config_streamer.h" + +#include "platform.h" + +#include + +#if !defined(FLASH_PAGE_SIZE) +# if defined(STM32F10X_MD) +# define FLASH_PAGE_SIZE (0x400) +# elif defined(STM32F10X_HD) +# define FLASH_PAGE_SIZE (0x800) +# elif defined(STM32F303xC) +# define FLASH_PAGE_SIZE (0x800) +# elif defined(STM32F40_41xxx) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) +# elif defined (STM32F411xE) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) +# elif defined(STM32F745xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x40000) +# elif defined(STM32F746xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x40000) +# elif defined(UNIT_TEST) +# define FLASH_PAGE_SIZE (0x400) +# else +# error "Flash page size not defined for target." +# endif +#endif + +void config_streamer_init(config_streamer_t *c) +{ + memset(c, 0, sizeof(*c)); +} + +void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) +{ + // base must start at FLASH_PAGE_SIZE boundary + c->address = base; + c->size = size; + if (!c->unlocked) { +#if defined(STM32F7) + HAL_FLASH_Unlock(); +#else + FLASH_Unlock(); +#endif + c->unlocked = true; + } + +#if defined(STM32F10X) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); +#elif defined(STM32F303) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); +#elif defined(STM32F4) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); +#elif defined(STM32F7) + // NOP +#elif defined(UNIT_TEST) + // NOP +#else +# error "Unsupported CPU" +#endif + c->err = 0; +} + +#if defined(STM32F7) +static int write_word(config_streamer_t *c, uint32_t value) +{ + if (c->err != 0) { + return c->err; + } + + HAL_StatusTypeDef status; + if (c->address % FLASH_PAGE_SIZE == 0) { + FLASH_EraseInitTypeDef EraseInitStruct = {0}; + EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; + EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V + EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1); + EraseInitStruct.NbSectors = 1; + uint32_t SECTORError; + status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); + if (status != HAL_OK){ + return -1; + } + } + status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value); + if (status != HAL_OK) { + return -2; + } + c->address += sizeof(value); + return 0; +} +#else +static int write_word(config_streamer_t *c, uint32_t value) +{ + if (c->err != 0) { + return c->err; + } + + FLASH_Status status; + + if (c->address % FLASH_PAGE_SIZE == 0) { +#if defined(STM32F40_41xxx) + status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 +#elif defined (STM32F411xE) + status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 +#else + status = FLASH_ErasePage(c->address); +#endif + if (status != FLASH_COMPLETE) { + return -1; + } + } + status = FLASH_ProgramWord(c->address, value); + if (status != FLASH_COMPLETE) { + return -2; + } + c->address += sizeof(value); + return 0; +} +#endif + +int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size) +{ + for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) { + c->buffer.b[c->at++] = *pat; + + if (c->at == sizeof(c->buffer)) { + c->err = write_word(c, c->buffer.w); + c->at = 0; + } + } + return c->err; +} + +int config_streamer_status(config_streamer_t *c) +{ + return c->err; +} + +int config_streamer_flush(config_streamer_t *c) +{ + if (c->at != 0) { + memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at); + c->err = write_word(c, c->buffer.w); + c->at = 0; + } + return c-> err; +} + +int config_streamer_finish(config_streamer_t *c) +{ + if (c->unlocked) { +#if defined(STM32F7) + HAL_FLASH_Lock(); +#else + FLASH_Lock(); +#endif + c->unlocked = false; + } + return c->err; +} diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h new file mode 100644 index 000000000..a62356fbe --- /dev/null +++ b/src/main/config/config_streamer.h @@ -0,0 +1,45 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +// Streams data out to the EEPROM, padding to the write size as +// needed, and updating the checksum as it goes. + +typedef struct config_streamer_s { + uintptr_t address; + int size; + union { + uint8_t b[4]; + uint32_t w; + } buffer; + int at; + int err; + bool unlocked; +} config_streamer_t; + +void config_streamer_init(config_streamer_t *c); + +void config_streamer_start(config_streamer_t *c, uintptr_t base, int size); +int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size); +int config_streamer_flush(config_streamer_t *c); + +int config_streamer_finish(config_streamer_t *c); +int config_streamer_status(config_streamer_t *c); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e4d2b69be..8ceecd622 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -91,6 +91,7 @@ #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" #ifndef DEFAULT_RX_FEATURE #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM @@ -1129,12 +1130,40 @@ void validateAndFixGyroConfig(void) } } +void readEEPROM(void) +{ + suspendRxSignal(); + + // Sanity check, read flash + if (!loadEEPROM()) { + failureMode(FAILURE_INVALID_EEPROM_CONTENTS); + } + + pgActivateProfile(getCurrentProfile()); + +// setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); + setControlRateProfile(0); + + validateAndFixConfig(); + activateConfig(); + + resumeRxSignal(); +} + +void writeEEPROM(void) +{ + suspendRxSignal(); + + writeConfigToEEPROM(); + + resumeRxSignal(); +} + void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { return; } - resetEEPROM(); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0fe4f9e61..4a77447df 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -69,7 +69,10 @@ void setPreferredBeeperOffMask(uint32_t mask); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); +void initEEPROM(void); void resetEEPROM(void); +void readEEPROM(void); +void writeEEPROM(); void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); @@ -80,6 +83,8 @@ void activateConfig(void); uint8_t getCurrentProfile(void); void changeProfile(uint8_t profileIndex); void setProfile(uint8_t profileIndex); +struct profile_s; +void resetProfile(struct profile_s *profile); uint8_t getCurrentControlRateProfile(void); void changeControlRateProfile(uint8_t profileIndex); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6b3b86a87..6de708dc6 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -105,7 +105,6 @@ #endif extern uint16_t cycleTime; // FIXME dependency on mw.c -extern void resetProfile(profile_t *profile); static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld index 2cc81c9fd..eaf65e7b9 100644 --- a/src/main/target/link/stm32_flash.ld +++ b/src/main/target/link/stm32_flash.ld @@ -81,6 +81,19 @@ SECTIONS KEEP (*(SORT(.fini_array.*))) PROVIDE_HIDDEN (__fini_array_end = .); } >FLASH + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH /* used by the startup to initialize data */ _sidata = .; From dd87908a564e03697359206f243e89118dfe7de0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Dec 2016 13:16:43 +0000 Subject: [PATCH 42/55] Fixed typo --- src/main/config/config_reset.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config_reset.h b/src/main/config/config_reset.h index 7ca0eda77..5d13e1247 100644 --- a/src/main/config/config_reset.h +++ b/src/main/config/config_reset.h @@ -32,7 +32,7 @@ memcpy((_name), &__UNIQL(_reset_template_), sizeof(*(_name))); \ /**/ -// overwrite _name with data passed as arguments. GCC is alloved to set structure field-by-field +// overwrite _name with data passed as arguments. GCC is allowed to set structure field-by-field #define RESET_CONFIG_2(_type, _name, ...) \ *(_name) = (_type) { \ __VA_ARGS__ \ From 87570f2dc9bf0e6755c727c933cc4f8b1c7d5372 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Dec 2016 17:15:16 +0000 Subject: [PATCH 43/55] Added dummy PG. Fixed setting of profile --- src/main/config/config_eeprom.c | 9 +++++++++ src/main/config/parameter_group_ids.h | 2 +- src/main/fc/config.c | 12 ++++++++---- src/main/fc/config.h | 3 +-- 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 18cbfa8e9..2850700c5 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -35,6 +35,15 @@ #include "fc/config.h" +// declare a dummy PG, since scanEEPROM assumes there is at least one PG +// !!TODO remove once first PG has been created out of masterConfg +typedef struct dummpConfig_s { + uint8_t dummy; +} dummyConfig_t; +PG_DECLARE(dummyConfig_t, dummyConfig); +#define PG_DUMMY_CONFIG 1 +PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0); + extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index c42ef32f0..fbef2e84d 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -16,7 +16,7 @@ */ // FC configuration -#define PG_FAILSAFE_CONFIG 1 // strruct OK +#define PG_FAILSAFE_CONFIG 1 // struct OK #define PG_BOARD_ALIGNMENT 2 // struct OK #define PG_GIMBAL_CONFIG 3 // struct OK #define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8ceecd622..e65ef76b2 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -550,7 +550,7 @@ uint8_t getCurrentProfile(void) return masterConfig.current_profile_index; } -void setProfile(uint8_t profileIndex) +static void setProfile(uint8_t profileIndex) { currentProfile = &masterConfig.profile[profileIndex]; currentControlRateProfileIndex = currentProfile->activeRateProfile; @@ -1139,10 +1139,14 @@ void readEEPROM(void) failureMode(FAILURE_INVALID_EEPROM_CONTENTS); } - pgActivateProfile(getCurrentProfile()); - +// pgActivateProfile(getCurrentProfile()); // setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); - setControlRateProfile(0); + + if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check + masterConfig.current_profile_index = 0; + } + + setProfile(masterConfig.current_profile_index); validateAndFixConfig(); activateConfig(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 4a77447df..62841f5fd 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -82,7 +82,6 @@ void activateConfig(void); uint8_t getCurrentProfile(void); void changeProfile(uint8_t profileIndex); -void setProfile(uint8_t profileIndex); struct profile_s; void resetProfile(struct profile_s *profile); @@ -91,7 +90,7 @@ void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); -struct master_s; +struct master_s; void targetConfiguration(struct master_s *config); void targetValidateConfiguration(struct master_s *config); From 9e529a28cad88787e1e997ce0c5a94c512c9800f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 2 Jan 2017 17:03:17 +0000 Subject: [PATCH 44/55] Updates as per iNav --- src/main/config/config_streamer.c | 138 +++++++++++++++++++++++------- src/main/drivers/system.h | 3 + src/main/fc/cli.c | 99 ++++++++++++++++----- src/main/fc/config.c | 7 +- src/main/fc/config.h | 1 + 5 files changed, 195 insertions(+), 53 deletions(-) diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index cd48ea9f7..e63363f22 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -15,11 +15,16 @@ * along with Cleanflight. If not, see . */ -#include "config_streamer.h" +#include #include "platform.h" -#include +#include "drivers/system.h" + +#include "config/config_streamer.h" + +extern uint8_t __config_start; // configured via linker script when building binaries. +extern uint8_t __config_end; #if !defined(FLASH_PAGE_SIZE) # if defined(STM32F10X_MD) @@ -32,6 +37,8 @@ # define FLASH_PAGE_SIZE ((uint32_t)0x20000) # elif defined (STM32F411xE) # define FLASH_PAGE_SIZE ((uint32_t)0x20000) +# elif defined(STM32F427_437xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) // 128K sectors # elif defined(STM32F745xx) # define FLASH_PAGE_SIZE ((uint32_t)0x40000) # elif defined(STM32F746xx) @@ -79,61 +86,134 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) } #if defined(STM32F7) +/* +Sector 0 0x08000000 - 0x08007FFF 32 Kbytes +Sector 1 0x08008000 - 0x0800FFFF 32 Kbytes +Sector 2 0x08010000 - 0x08017FFF 32 Kbytes +Sector 3 0x08018000 - 0x0801FFFF 32 Kbytes +Sector 4 0x08020000 - 0x0803FFFF 128 Kbytes +Sector 5 0x08040000 - 0x0807FFFF 256 Kbytes +Sector 6 0x08080000 - 0x080BFFFF 256 Kbytes +Sector 7 0x080C0000 - 0x080FFFFF 256 Kbytes +*/ + +static uint32_t getFLASHSectorForEEPROM(void) +{ + if ((uint32_t)&__config_start <= 0x08007FFF) + return FLASH_SECTOR_0; + if ((uint32_t)&__config_start <= 0x0800FFFF) + return FLASH_SECTOR_1; + if ((uint32_t)&__config_start <= 0x08017FFF) + return FLASH_SECTOR_2; + if ((uint32_t)&__config_start <= 0x0801FFFF) + return FLASH_SECTOR_3; + if ((uint32_t)&__config_start <= 0x0803FFFF) + return FLASH_SECTOR_4; + if ((uint32_t)&__config_start <= 0x0807FFFF) + return FLASH_SECTOR_5; + if ((uint32_t)&__config_start <= 0x080BFFFF) + return FLASH_SECTOR_6; + if ((uint32_t)&__config_start <= 0x080FFFFF) + return FLASH_SECTOR_7; + + // Not good + while (1) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } +} + +#elif defined(STM32F4) +/* +Sector 0 0x08000000 - 0x08003FFF 16 Kbytes +Sector 1 0x08004000 - 0x08007FFF 16 Kbytes +Sector 2 0x08008000 - 0x0800BFFF 16 Kbytes +Sector 3 0x0800C000 - 0x0800FFFF 16 Kbytes +Sector 4 0x08010000 - 0x0801FFFF 64 Kbytes +Sector 5 0x08020000 - 0x0803FFFF 128 Kbytes +Sector 6 0x08040000 - 0x0805FFFF 128 Kbytes +Sector 7 0x08060000 - 0x0807FFFF 128 Kbytes +Sector 8 0x08080000 - 0x0809FFFF 128 Kbytes +Sector 9 0x080A0000 - 0x080BFFFF 128 Kbytes +Sector 10 0x080C0000 - 0x080DFFFF 128 Kbytes +Sector 11 0x080E0000 - 0x080FFFFF 128 Kbytes +*/ + +static uint32_t getFLASHSectorForEEPROM(void) +{ + if ((uint32_t)&__config_start <= 0x08003FFF) + return FLASH_Sector_0; + if ((uint32_t)&__config_start <= 0x08007FFF) + return FLASH_Sector_1; + if ((uint32_t)&__config_start <= 0x0800BFFF) + return FLASH_Sector_2; + if ((uint32_t)&__config_start <= 0x0800FFFF) + return FLASH_Sector_3; + if ((uint32_t)&__config_start <= 0x0801FFFF) + return FLASH_Sector_4; + if ((uint32_t)&__config_start <= 0x0803FFFF) + return FLASH_Sector_5; + if ((uint32_t)&__config_start <= 0x0805FFFF) + return FLASH_Sector_6; + if ((uint32_t)&__config_start <= 0x0807FFFF) + return FLASH_Sector_7; + if ((uint32_t)&__config_start <= 0x0809FFFF) + return FLASH_Sector_8; + if ((uint32_t)&__config_start <= 0x080DFFFF) + return FLASH_Sector_9; + if ((uint32_t)&__config_start <= 0x080BFFFF) + return FLASH_Sector_10; + if ((uint32_t)&__config_start <= 0x080FFFFF) + return FLASH_Sector_11; + + // Not good + while (1) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } +} +#endif + static int write_word(config_streamer_t *c, uint32_t value) { if (c->err != 0) { return c->err; } - - HAL_StatusTypeDef status; +#if defined(STM32F7) if (c->address % FLASH_PAGE_SIZE == 0) { - FLASH_EraseInitTypeDef EraseInitStruct = {0}; - EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; - EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; // 2.7-3.6V - EraseInitStruct.Sector = (FLASH_SECTOR_TOTAL-1); - EraseInitStruct.NbSectors = 1; + FLASH_EraseInitTypeDef EraseInitStruct = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V + .NbSectors = 1 + }; + EraseInitStruct.Sector = getFLASHSectorForEEPROM(); uint32_t SECTORError; - status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); + const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError); if (status != HAL_OK){ return -1; } } - status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value); + const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, c->address, value); if (status != HAL_OK) { return -2; } - c->address += sizeof(value); - return 0; -} #else -static int write_word(config_streamer_t *c, uint32_t value) -{ - if (c->err != 0) { - return c->err; - } - - FLASH_Status status; - if (c->address % FLASH_PAGE_SIZE == 0) { -#if defined(STM32F40_41xxx) - status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 -#elif defined (STM32F411xE) - status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 +#if defined(STM32F4) + const FLASH_Status status = FLASH_EraseSector(getFLASHSectorForEEPROM(), VoltageRange_3); //0x08080000 to 0x080A0000 #else - status = FLASH_ErasePage(c->address); + const FLASH_Status status = FLASH_ErasePage(c->address); #endif if (status != FLASH_COMPLETE) { return -1; } } - status = FLASH_ProgramWord(c->address, value); + const FLASH_Status status = FLASH_ProgramWord(c->address, value); if (status != FLASH_COMPLETE) { return -2; } +#endif c->address += sizeof(value); return 0; } -#endif int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size) { diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 87af44df5..5ac340c31 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + void systemInit(void); void delayMicroseconds(uint32_t us); void delay(uint32_t ms); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 87ca6b2e1..abb8dc147 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -23,6 +23,7 @@ #include #include +//#define USE_PARAMETER_GROUPS #include "platform.h" // FIXME remove this for targets that don't need a CLI. Perhaps use a no-op macro when USE_CLI is not enabled @@ -48,6 +49,9 @@ uint8_t cliMode = 0; #include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/buf_writer.h" #include "drivers/bus_i2c.h" @@ -460,6 +464,7 @@ typedef enum { MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), + CONTROL_RATE_VALUE = (3 << VALUE_SECTION_OFFSET), // value mode MODE_DIRECT = (0 << VALUE_MODE_OFFSET), MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) @@ -483,6 +488,22 @@ typedef union { cliMinMaxConfig_t minmax; } cliValueConfig_t; +#ifdef USE_PARAMETER_GROUPS +typedef struct { + const char *name; + const uint8_t type; // see cliValueFlag_e + const cliValueConfig_t config; + + pgn_t pgn; + uint16_t offset; +} __attribute__((packed)) clivalue_t; + +static const clivalue_t valueTable[] = { + { "dummy", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, 0, 0 } +}; + +#else + typedef struct { const char *name; const uint8_t type; // see cliValueFlag_e @@ -490,7 +511,7 @@ typedef struct { const cliValueConfig_t config; } clivalue_t; -const clivalue_t valueTable[] = { +static const clivalue_t valueTable[] = { #ifndef SKIP_TASK_STATISTICS { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.task_statistics, .config.lookup = { TABLE_OFF_ON } }, #endif @@ -811,8 +832,7 @@ const clivalue_t valueTable[] = { { "displayport_max7456_row_adjust", VAR_INT8 | MASTER_VALUE, &displayPortProfileMax7456()->rowAdjust, .config.minmax = { -3, 0 } }, #endif }; - -#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) +#endif static void cliPrint(const char *str) { @@ -931,6 +951,24 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_ } } +#ifdef USE_PARAMETER_GROUPS +static void* getValuePointer(const clivalue_t *var) +{ + const pgRegistry_t* rec = pgFind(var->pgn); + + switch (var->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + return rec->address + var->offset; + case PROFILE_RATE_VALUE: + return rec->address + var->offset + sizeof(profile_t) * getCurrentProfile(); + case CONTROL_RATE_VALUE: + return rec->address + var->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + case PROFILE_VALUE: + return *rec->ptr + var->offset; + } + return NULL; +} +#else void *getValuePointer(const clivalue_t *value) { void *ptr = value->ptr; @@ -945,6 +983,7 @@ void *getValuePointer(const clivalue_t *value) return ptr; } +#endif static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig) { @@ -1006,7 +1045,7 @@ static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const maste static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig) { const clivalue_t *value; - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { value = &valueTable[i]; if ((value->type & VALUE_SECTION_MASK) != valueSection) { @@ -1053,13 +1092,7 @@ typedef union { static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { - void *ptr = var->ptr; - if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); - } - if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); - } + void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -3074,7 +3107,7 @@ static void cliGet(char *cmdline) const clivalue_t *val; int matchedCommands = 0; - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { if (strstr(valueTable[i].name, cmdline)) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); @@ -3105,7 +3138,7 @@ static void cliSet(char *cmdline) if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrint("Current settings: \r\n"); - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui @@ -3126,7 +3159,7 @@ static void cliSet(char *cmdline) eqptr++; } - for (uint32_t i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { val = &valueTable[i]; // ensure exact match when setting to prevent setting variables with shorter names if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { @@ -3529,6 +3562,18 @@ static void cliResource(char *cmdline) } #endif /* USE_RESOURCE_MGMT */ +#ifdef USE_PARAMETER_GROUPS +static void backupConfigs(void) +{ + // make copies of configs to do differencing + +} + +static void restoreConfigs(void) +{ +} +#endif + static void printConfig(char *cmdline, bool doDiff) { uint8_t dumpMask = DUMP_MASTER; @@ -3545,13 +3590,21 @@ static void printConfig(char *cmdline, bool doDiff) options = cmdline; } - static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; } + static master_t defaultConfig; createDefaultConfig(&defaultConfig); +#ifdef USE_PARAMETER_GROUPS + backupConfigs(); + // reset all configs to defaults to do differencing + resetConfigs(); +#if defined(TARGET_CONFIG) + targetConfiguration(&defaultConfig); +#endif +#endif if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values } @@ -3674,6 +3727,10 @@ static void printConfig(char *cmdline, bool doDiff) if (dumpMask & DUMP_RATES) { cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } +#ifdef USE_PARAMETER_GROUPS + // restore configs from copies + restoreConfigs(); +#endif } static void cliDump(char *cmdline) @@ -3800,13 +3857,11 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif }; -#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) - static void cliHelp(char *cmdline) { UNUSED(cmdline); - for (uint32_t i = 0; i < CMD_COUNT; i++) { + for (uint32_t i = 0; i < ARRAYLEN(cmdTable); i++) { cliPrint(cmdTable[i].name); #ifndef MINIMAL_CLI if (cmdTable[i].description) { @@ -3835,7 +3890,7 @@ void cliProcess(void) // do tab completion const clicmd_t *cmd, *pstart = NULL, *pend = NULL; uint32_t i = bufferIndex; - for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0)) continue; if (!pstart) @@ -3896,12 +3951,12 @@ void cliProcess(void) const clicmd_t *cmd; char *options; - for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { + for (cmd = cmdTable; cmd < cmdTable + ARRAYLEN(cmdTable); cmd++) { if ((options = checkCommand(cliBuffer, cmd->name))) { break; - } + } } - if(cmd < cmdTable + CMD_COUNT) + if(cmd < cmdTable + ARRAYLEN(cmdTable)) cmd->func(options); else cliPrint("Unknown command, try 'help'"); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e65ef76b2..e66c6e139 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -872,11 +872,14 @@ void createDefaultConfig(master_t *config) } } -static void resetConf(void) +void resetConfigs(void) { createDefaultConfig(&masterConfig); + pgResetAll(MAX_PROFILE_COUNT); + pgActivateProfile(0); setProfile(0); + setControlRateProfile(0); #ifdef LED_STRIP reevaluateLedConfig(); @@ -1173,7 +1176,7 @@ void ensureEEPROMContainsValidData(void) void resetEEPROM(void) { - resetConf(); + resetConfigs(); writeEEPROM(); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 62841f5fd..6466507e6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -91,6 +91,7 @@ bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); +void resetConfigs(void); struct master_s; void targetConfiguration(struct master_s *config); void targetValidateConfiguration(struct master_s *config); From 83343e1d86f72da485c598d982d1b0ac939097bc Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 10:26:25 +0000 Subject: [PATCH 45/55] Preparation for doing PG differencing in CLI --- src/main/config/parameter_group.h | 30 ++-- src/main/fc/cli.c | 220 +++++++++++++++++++++--------- 2 files changed, 175 insertions(+), 75 deletions(-) diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 734933ca9..2533b5015 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -20,6 +20,8 @@ #include #include +#include "build/build_config.h" + typedef uint16_t pgn_t; // parameter group registry flags @@ -54,6 +56,7 @@ static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_M static inline uint8_t pgVersion(const pgRegistry_t* reg) {return reg->pgn >> 12;} static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;} static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;} +static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == PGR_SIZE_PROFILE_FLAG;} #define PG_PACKED __attribute__((packed)) @@ -100,22 +103,25 @@ extern const uint8_t __pg_resetdata_end[]; // Declare system config #define PG_DECLARE(_type, _name) \ extern _type _name ## _System; \ - static inline _type* _name(void) { return &_name ## _System; } \ + static inline const _type* _name(void) { return &_name ## _System; }\ + static inline _type* _name ## Mutable(void) { return &_name ## _System; }\ struct _dummy \ /**/ // Declare system config array -#define PG_DECLARE_ARR(_type, _size, _name) \ +#define PG_DECLARE_ARRAY(_type, _size, _name) \ extern _type _name ## _SystemArray[_size]; \ - static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ - static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \ + static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline _type (* _name ## _array(void))[_size] { return &_name ## _SystemArray; } \ struct _dummy \ /**/ // Declare profile config #define PG_DECLARE_PROFILE(_type, _name) \ extern _type *_name ## _ProfileCurrent; \ - static inline _type* _name(void) { return _name ## _ProfileCurrent; } \ + static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \ + static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \ struct _dummy \ /**/ @@ -148,7 +154,7 @@ extern const uint8_t __pg_resetdata_end[]; /**/ // Register system config array -#define PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, _reset) \ +#define PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, _reset) \ _type _name ## _SystemArray[_size]; \ extern const pgRegistry_t _name ##_Registry; \ const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \ @@ -160,20 +166,20 @@ extern const uint8_t __pg_resetdata_end[]; } \ /**/ -#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version) \ - PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \ +#define PG_REGISTER_ARRAY(_type, _size, _name, _pgn, _version) \ + PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \ /**/ -#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \ +#define PG_REGISTER_ARRAY_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \ extern void pgResetFn_ ## _name(_type *); \ - PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ + PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ /**/ #if 0 // ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it - See pgResetInstance -#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \ +#define PG_REGISTER_ARRAY_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \ extern const _type pgResetTemplate_ ## _name; \ - PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + PG_REGISTER_ARRAY_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ /**/ #endif diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index abb8dc147..4782d812e 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -117,16 +117,6 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; static char cliBuffer[48]; static uint32_t bufferIndex = 0; -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), - DO_DIFF = (1 << 4), - SHOW_DEFAULTS = (1 << 5), - HIDE_UNUSED = (1 << 6) -} dumpFlags_e; - static const char* const emptyName = "-"; #ifndef USE_QUAD_MIXER_ONLY @@ -452,22 +442,21 @@ static const lookupTableEntry_t lookupTables[] = { #define VALUE_MODE_OFFSET 6 typedef enum { - // value type + // value type, bits 0-3 VAR_UINT8 = (0 << VALUE_TYPE_OFFSET), VAR_INT8 = (1 << VALUE_TYPE_OFFSET), VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), //VAR_UINT32 = (4 << VALUE_TYPE_OFFSET), - VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), + VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), // 0x05 - // value section + // value section, bits 4-5 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), - PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), - CONTROL_RATE_VALUE = (3 << VALUE_SECTION_OFFSET), + PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20 // value mode - MODE_DIRECT = (0 << VALUE_MODE_OFFSET), - MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) + MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40 + MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80 } cliValueFlag_e; #define VALUE_TYPE_MASK (0x0F) @@ -858,6 +847,16 @@ static void cliPutp(void *p, char ch) bufWriterAppend(p, ch); } +typedef enum { + DUMP_MASTER = (1 << 0), + DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), + DO_DIFF = (1 << 4), + SHOW_DEFAULTS = (1 << 5), + HIDE_UNUSED = (1 << 6) +} dumpFlags_e; + static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...) { if (!((dumpMask & DO_DIFF) && equalsDefault)) { @@ -952,23 +951,121 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_ } #ifdef USE_PARAMETER_GROUPS -static void* getValuePointer(const clivalue_t *var) -{ - const pgRegistry_t* rec = pgFind(var->pgn); - switch (var->type & VALUE_SECTION_MASK) { +static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault) +{ + bool result = false; + switch (type & VALUE_TYPE_MASK) { + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; + + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; + + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; + + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + +/* not currently used + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break;*/ + + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; + } + return result; +} + +typedef struct cliCurrentAndDefaultConfig_s { + const void *currentConfig; // the copy + const void *defaultConfig; // the PG value as set by default +} cliCurrentAndDefaultConfig_t; + +static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn) +{ + static cliCurrentAndDefaultConfig_t ret; + + switch (pgn) { + default: + ret.currentConfig = NULL; + ret.defaultConfig = NULL; + break; + } + return &ret; +} + +static uint16_t getValueOffset(const clivalue_t *value) +{ + switch (value->type & VALUE_SECTION_MASK) { case MASTER_VALUE: - return rec->address + var->offset; - case PROFILE_RATE_VALUE: - return rec->address + var->offset + sizeof(profile_t) * getCurrentProfile(); - case CONTROL_RATE_VALUE: - return rec->address + var->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); case PROFILE_VALUE: - return *rec->ptr + var->offset; + return value->offset; + case PROFILE_RATE_VALUE: + return value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); + } + return 0; +} + +static void *getValuePointer(const clivalue_t *value) +{ + const pgRegistry_t* rec = pgFind(value->pgn); + + switch (value->type & VALUE_SECTION_MASK) { + case MASTER_VALUE: + case PROFILE_VALUE: + return rec->address + value->offset; + case PROFILE_RATE_VALUE: + return rec->address + value->offset + sizeof(controlRateConfig_t) * getCurrentControlRateProfile(); } return NULL; } + +static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) +{ + const char *format = "set %s = "; + const cliCurrentAndDefaultConfig_t *config = getCurrentAndDefaultConfigs(value->pgn); + if (config->currentConfig == NULL || config->defaultConfig == NULL) { + // has not been set up properly + cliPrintf("VALUE %s ERROR\r\n", value->name); + return; + } + const int valueOffset = getValueOffset(value); + switch (dumpMask & (DO_DIFF | SHOW_DEFAULTS)) { + case DO_DIFF: + if (valuePtrEqualsDefault(value->type, (uint8_t*)config->currentConfig + valueOffset, (uint8_t*)config->defaultConfig + valueOffset)) { + break; + } + // drop through, since not equal to default + case 0: + case SHOW_DEFAULTS: + cliPrintf(format, value->name); + printValuePointer(value, (uint8_t*)config->currentConfig + valueOffset, 0); + cliPrint("\r\n"); + break; + } +} + +static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) +{ + for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) { + const clivalue_t *value = &valueTable[i]; + bufWriterFlush(cliWriter); + if ((value->type & VALUE_SECTION_MASK) == valueSection) { + dumpPgValue(value, dumpMask); + } + } +} + #else + void *getValuePointer(const clivalue_t *value) { void *ptr = value->ptr; @@ -1141,9 +1238,9 @@ static void cliShowArgumentRangeError(char *name, int min, int max) cliPrintf("%s not between %d and %d\r\n", name, min, max); } -static char *nextArg(char *currentArg) +static const char *nextArg(const char *currentArg) { - char *ptr = strchr(currentArg, ' '); + const char *ptr = strchr(currentArg, ' '); while (ptr && *ptr == ' ') { ptr++; } @@ -1151,14 +1248,12 @@ static char *nextArg(char *currentArg) return ptr; } -static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount) +static const char *processChannelRangeArgs(const char *ptr, channelRange_t *range, uint8_t *validArgumentCount) { - int val; - for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { ptr = nextArg(ptr); if (ptr) { - val = atoi(ptr); + int val = atoi(ptr); val = CHANNEL_VALUE_TO_STEP(val); if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { if (argIndex == 0) { @@ -1177,10 +1272,10 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t * // Check if a string's length is zero static bool isEmpty(const char *string) { - return *string == '\0'; + return (string == NULL || *string == '\0') ? true : false; } -static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) +static void printRxFailsafe(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) { // print out rxConfig failsafe settings for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { @@ -1219,7 +1314,7 @@ static void printRxFail(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxCo } } -static void cliRxFail(char *cmdline) +static void cliRxFailsafe(char *cmdline) { uint8_t channel; char buf[3]; @@ -1227,10 +1322,10 @@ static void cliRxFail(char *cmdline) if (isEmpty(cmdline)) { // print out rxConfig failsafe settings for (channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { - cliRxFail(itoa(channel, buf, 10)); + cliRxFailsafe(itoa(channel, buf, 10)); } } else { - char *ptr = cmdline; + const char *ptr = cmdline; channel = atoi(ptr++); if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) { @@ -1337,7 +1432,7 @@ static void printAux(uint8_t dumpMask, const modeActivationProfile_t *modeActiva static void cliAux(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printAux(DUMP_MASTER, modeActivationProfile(), NULL); @@ -1411,13 +1506,9 @@ static void printSerial(uint8_t dumpMask, const serialConfig_t *serialConfig, co static void cliSerial(char *cmdline) { - int i, val; - char *ptr; - if (isEmpty(cmdline)) { printSerial(DUMP_MASTER, serialConfig(), NULL); - - return; + return; } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1426,9 +1517,9 @@ static void cliSerial(char *cmdline) uint8_t validArgumentCount = 0; - ptr = cmdline; + const char *ptr = cmdline; - val = atoi(ptr++); + int val = atoi(ptr++); currentConfig = serialFindPortConfiguration(val); if (currentConfig) { portConfig.identifier = val; @@ -1442,7 +1533,7 @@ static void cliSerial(char *cmdline) validArgumentCount++; } - for (i = 0; i < 4; i ++) { + for (int i = 0; i < 4; i ++) { ptr = nextArg(ptr); if (!ptr) { break; @@ -1491,7 +1582,6 @@ static void cliSerial(char *cmdline) } memcpy(currentConfig, &portConfig, sizeof(portConfig)); - } #ifndef SKIP_SERIAL_PASSTHROUGH @@ -1609,7 +1699,7 @@ static void printAdjustmentRange(uint8_t dumpMask, const adjustmentProfile_t *ad static void cliAdjustmentRange(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printAdjustmentRange(DUMP_MASTER, adjustmentProfile(), NULL); @@ -1713,7 +1803,7 @@ static void cliMotorMix(char *cmdline) #else int check = 0; uint8_t len; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); @@ -1801,7 +1891,7 @@ static void printRxRange(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxC static void cliRxRange(char *cmdline) { int i, validArgumentCount = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printRxRange(DUMP_MASTER, rxConfig(), NULL); @@ -1863,7 +1953,7 @@ static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledC static void cliLed(char *cmdline) { int i; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printLed(DUMP_MASTER, ledStripConfig()->ledConfigs, NULL); @@ -1901,7 +1991,7 @@ static void printColor(uint8_t dumpMask, const hsvColor_t *colors, const hsvColo static void cliColor(char *cmdline) { int i; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printColor(DUMP_MASTER, ledStripConfig()->colors, NULL); @@ -2179,10 +2269,8 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) static void cliServoMix(char *cmdline) { - uint8_t len; - char *ptr; int args[8], check = 0; - len = strlen(cmdline); + int len = strlen(cmdline); if (len == 0) { printServoMix(DUMP_MASTER, NULL); @@ -2193,7 +2281,7 @@ static void cliServoMix(char *cmdline) servoProfile()->servoConf[i].reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = nextArg(cmdline); + const char *ptr = nextArg(cmdline); if (ptr) { len = strlen(ptr); for (uint32_t i = 0; ; i++) { @@ -2211,7 +2299,7 @@ static void cliServoMix(char *cmdline) } } else if (strncasecmp(cmdline, "reverse", 7) == 0) { enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; - ptr = strchr(cmdline, ' '); + char *ptr = strchr(cmdline, ' '); len = strlen(ptr); if (len == 0) { @@ -2253,7 +2341,7 @@ static void cliServoMix(char *cmdline) cliServoMix("reverse"); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; - ptr = strtok(cmdline, " "); + char *ptr = strtok(cmdline, " "); while (ptr != NULL && check < ARGS_COUNT) { args[check++] = atoi(ptr); ptr = strtok(NULL, " "); @@ -2427,7 +2515,7 @@ static void cliFlashRead(char *cmdline) uint8_t buffer[32]; - char *nextArg = strchr(cmdline, ' '); + const char *nextArg = strchr(cmdline, ' '); if (!nextArg) { cliShowParseError(); @@ -2496,7 +2584,7 @@ static void printVtx(uint8_t dumpMask, const master_t *defaultConfig) static void cliVtx(char *cmdline) { int i, val = 0; - char *ptr; + const char *ptr; if (isEmpty(cmdline)) { printVtx(DUMP_MASTER, NULL); @@ -3067,7 +3155,13 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_ cliPrintHashLine("profile"); cliProfile(""); cliPrint("\r\n"); +#ifdef USE_PARAMETER_GROUPS + (void)(defaultConfig); + dumpAllValues(PROFILE_VALUE, dumpMask); + dumpAllValues(PROFILE_RATE_VALUE, dumpMask); +#else dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); +#endif cliRateProfile(""); } @@ -3688,7 +3782,7 @@ static void printConfig(char *cmdline, bool doDiff) #endif cliPrintHashLine("rxfail"); - printRxFail(dumpMask, rxConfig(), &defaultConfig.rxConfig); + printRxFailsafe(dumpMask, rxConfig(), &defaultConfig.rxConfig); cliPrintHashLine("master"); dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); @@ -3828,7 +3922,7 @@ const clicmd_t cmdTable[] = { #if defined(USE_RESOURCE_MGMT) CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource), #endif - CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), + CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFailsafe), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), #ifdef USE_SDCARD From 4dee6b7dddf27e2e39f97b8d00e7d9a206db5228 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 11:34:20 +0000 Subject: [PATCH 46/55] Fixed up unit tests and ROM sizes --- src/main/target/NAZE/target.h | 3 +-- src/test/unit/parameter_groups_unittest.cc | 10 ++++------ 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index b9cc396a3..315faf2fe 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -101,8 +101,7 @@ #define ACC_MPU6500_ALIGN CW0_DEG #define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 +#define USE_BARO_MS5611 // needed for Flip32 board #define USE_BARO_BMP280 /* diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc index df45a1829..a48e98e05 100644 --- a/src/test/unit/parameter_groups_unittest.cc +++ b/src/test/unit/parameter_groups_unittest.cc @@ -22,10 +22,8 @@ #include extern "C" { - #include "build/debug.h" - #include - + #include "build/debug.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -49,7 +47,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, TEST(ParameterGroupsfTest, Test_pgResetAll) { - memset(motorConfig(), 0, sizeof(motorConfig_t)); + memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); pgResetAll(0); EXPECT_EQ(1150, motorConfig()->minthrottle); EXPECT_EQ(1850, motorConfig()->maxthrottle); @@ -59,7 +57,7 @@ TEST(ParameterGroupsfTest, Test_pgResetAll) TEST(ParameterGroupsfTest, Test_pgFind) { - memset(motorConfig(), 0, sizeof(motorConfig_t)); + memset(motorConfigMutable(), 0, sizeof(motorConfig_t)); const pgRegistry_t *pgRegistry = pgFind(PG_MOTOR_CONFIG); pgResetCurrent(pgRegistry); EXPECT_EQ(1150, motorConfig()->minthrottle); @@ -69,7 +67,7 @@ TEST(ParameterGroupsfTest, Test_pgFind) motorConfig_t motorConfig2; memset(&motorConfig2, 0, sizeof(motorConfig_t)); - motorConfig()->motorPwmRate = 500; + motorConfigMutable()->motorPwmRate = 500; pgStore(pgRegistry, &motorConfig2, sizeof(motorConfig_t), 0); EXPECT_EQ(1150, motorConfig2.minthrottle); EXPECT_EQ(1850, motorConfig2.maxthrottle); From 46fa01a4b82c1fa3adaab35ebf0716a234132c75 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 12:04:41 +0000 Subject: [PATCH 47/55] Added STM32F722xx FLASH page size --- src/main/config/config_streamer.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index e63363f22..c4b6f6af0 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -27,18 +27,24 @@ extern uint8_t __config_start; // configured via linker script when building b extern uint8_t __config_end; #if !defined(FLASH_PAGE_SIZE) +// F1 # if defined(STM32F10X_MD) # define FLASH_PAGE_SIZE (0x400) # elif defined(STM32F10X_HD) # define FLASH_PAGE_SIZE (0x800) +// F3 # elif defined(STM32F303xC) # define FLASH_PAGE_SIZE (0x800) +// F4 # elif defined(STM32F40_41xxx) # define FLASH_PAGE_SIZE ((uint32_t)0x20000) # elif defined (STM32F411xE) # define FLASH_PAGE_SIZE ((uint32_t)0x20000) # elif defined(STM32F427_437xx) # define FLASH_PAGE_SIZE ((uint32_t)0x20000) // 128K sectors +// F7 +#elif defined(STM32F722xx) +# define FLASH_PAGE_SIZE ((uint32_t)0x20000) # elif defined(STM32F745xx) # define FLASH_PAGE_SIZE ((uint32_t)0x40000) # elif defined(STM32F746xx) From 108d9d6b61b05d96163dfd4cdea2d4d86f413b8e Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Dec 2016 19:31:02 +0100 Subject: [PATCH 48/55] Fix atomic.h `=m` output operand means that value is write only, gcc may discard previous value because it assumes that it will be overwritten. This bug was not triggered in gcc v4 because `asm volatile` triggered full memory barrier. With old version, this code will increase `markme` only by 2, not 3: ``` static int markme = 0; markme++; ATOMIC_BLOCK_NB(0xff) { ATOMIC_BARRIER(markme); // markme is marked as overwritten, previous increment can be discarded markme++; } markme++; ``` --- src/main/build/atomic.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 959c6e9d8..f2436d553 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -104,7 +104,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) __asm__ volatile ("\t# barier(" #data ") end\n" : : "m" (**__d)); \ } \ typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ - __asm__ volatile ("\t# barier (" #data ") start\n" : "=m" (*__UNIQL(__barrier))) + __asm__ volatile ("\t# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier))) // define these wrappers for atomic operations, use gcc buildins From 0fb5128d4aec97955c03cd3cc2df1e76f53b6f57 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Fri, 30 Dec 2016 12:57:34 +0100 Subject: [PATCH 49/55] Improve ATOMIC_BARRIER documentation --- src/main/build/atomic.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index f2436d553..3708e911a 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -69,25 +69,25 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \ __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 ) -// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier. -// Be carefull when using this, you must use some method to prevent optimizer form breaking things -// - lto is used for baseflight compillation, so function call is not memory barrier -// - use ATOMIC_BARRIER or propper volatile to protect used variables -// - gcc 4.8.4 does write all values in registes to memory before 'asm volatile', so this optimization does not help much -// but that can change in future versions +// 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 +// - 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(), \ __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \ // ATOMIC_BARRIER // Create memory barrier -// - at the beginning (all data must be reread from memory) -// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use) -// ideally this would only protect memory passed as parameter (any type should work), but gcc is curently creating almost full 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) // 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" -// increment version number is BARRIER works +// 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 #endif From 7cdd3818577ad272c2b03d010b6febb5703c84c5 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 26 Jan 2017 10:05:17 +1300 Subject: [PATCH 50/55] Fixed typo in generated comment. --- src/main/build/atomic.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index 3708e911a..0508f77e4 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -101,10 +101,10 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) // this macro uses local function for cleanup. CLang block can be substituded #define ATOMIC_BARRIER(data) \ __extension__ void __UNIQL(__barrierEnd)(typeof(data) **__d) { \ - __asm__ volatile ("\t# barier(" #data ") end\n" : : "m" (**__d)); \ + __asm__ volatile ("\t# barrier(" #data ") end\n" : : "m" (**__d)); \ } \ typeof(data) __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \ - __asm__ volatile ("\t# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier))) + __asm__ volatile ("\t# barrier (" #data ") start\n" : "+m" (*__UNIQL(__barrier))) // define these wrappers for atomic operations, use gcc buildins From cdbbad2012fed66f7392c4a009ae5ffb42cc696b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 21:06:01 +0000 Subject: [PATCH 51/55] Alignment with iNav --- src/main/config/config_eeprom.c | 19 +++++---- src/main/config/config_eeprom.h | 4 ++ src/main/fc/config.c | 69 +++++++++++++++++---------------- src/main/fc/config.h | 3 ++ 4 files changed, 54 insertions(+), 41 deletions(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index 2850700c5..295cd77b9 100644 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -47,6 +47,8 @@ PG_REGISTER(dummyConfig_t, dummyConfig, PG_DUMMY_CONFIG, 0); extern uint8_t __config_start; // configured via linker script when building binaries. extern uint8_t __config_end; +static uint16_t eepromConfigSize; + typedef enum { CR_CLASSICATION_SYSTEM = 0, CR_CLASSICATION_PROFILE1 = 1, @@ -111,7 +113,7 @@ static uint8_t updateChecksum(uint8_t chk, const void *data, uint32_t length) } // Scan the EEPROM config. Returns true if the config is valid. -static bool scanEEPROM(void) +bool isEEPROMContentValid(void) { uint8_t chk = 0; const uint8_t *p = &__config_start; @@ -148,7 +150,15 @@ static bool scanEEPROM(void) chk = updateChecksum(chk, footer, sizeof(*footer)); p += sizeof(*footer); chk = ~chk; - return chk == *p; + const uint8_t checkSum = *p; + p += sizeof(checkSum); + eepromConfigSize = p - &__config_start; + return chk == checkSum; +} + +uint16_t getEEPROMConfigSize(void) +{ + return eepromConfigSize; } // find config record for reg + classification (profile info) in EEPROM @@ -207,11 +217,6 @@ bool loadEEPROM(void) return true; } -bool isEEPROMContentValid(void) -{ - return scanEEPROM(); -} - static bool writeSettingsToEEPROM(void) { config_streamer_t streamer; diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index a6aafc3ca..62c8c6181 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -17,8 +17,12 @@ #pragma once +#include +#include + #define EEPROM_CONF_VERSION 154 bool isEEPROMContentValid(void); bool loadEEPROM(void); void writeConfigToEEPROM(void); +uint16_t getEEPROMConfigSize(void); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e66c6e139..8ee0519c8 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -29,12 +29,18 @@ #include "cms/cms.h" -#include "common/color.h" #include "common/axis.h" -#include "common/maths.h" +#include "common/color.h" #include "common/filter.h" +#include "common/maths.h" + +#include "config/config_eeprom.h" +#include "config/config_master.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" -#include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/io.h" @@ -45,6 +51,7 @@ #include "drivers/rx_pwm.h" #include "drivers/rx_spi.h" #include "drivers/sdcard.h" +#include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/sound_beeper.h" #include "drivers/system.h" @@ -56,43 +63,37 @@ #include "fc/rc_curves.h" #include "fc/runtime_config.h" -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/boardalignment.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/navigation.h" +#include "flight/pid.h" +#include "flight/servos.h" #include "io/beeper.h" -#include "io/serial.h" #include "io/gimbal.h" -#include "io/motors.h" -#include "io/servos.h" -#include "io/ledstrip.h" #include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" #include "io/osd.h" +#include "io/serial.h" +#include "io/servos.h" #include "io/vtx.h" #include "rx/rx.h" #include "rx/rx_spi.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" + #include "telemetry/telemetry.h" -#include "flight/mixer.h" -#include "flight/servos.h" -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/failsafe.h" -#include "flight/altitudehold.h" -#include "flight/navigation.h" - -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" -#include "config/parameter_group.h" - #ifndef DEFAULT_RX_FEATURE #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM #endif @@ -1166,6 +1167,12 @@ void writeEEPROM(void) resumeRxSignal(); } +void resetEEPROM(void) +{ + resetConfigs(); + writeEEPROM(); +} + void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { @@ -1174,12 +1181,6 @@ void ensureEEPROMContainsValidData(void) resetEEPROM(); } -void resetEEPROM(void) -{ - resetConfigs(); - writeEEPROM(); -} - void saveConfigAndNotify(void) { writeEEPROM(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 6466507e6..7a85b8538 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -17,8 +17,11 @@ #pragma once +#include #include +#include "config/parameter_group.h" + #if FLASH_SIZE <= 128 #define MAX_PROFILE_COUNT 2 #else From 7ce3753eac35003c5309132378dc5bf4f7e2c7ef Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 26 Jan 2017 11:33:29 +0900 Subject: [PATCH 52/55] Fix TARGET_IO_PORTx defs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit So bogus ports don’t show up on “resource list”. --- src/main/target/OMNIBUSF4/target.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 6d4289da5..e97ff6468 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -173,10 +173,10 @@ #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 0xffff +#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)|BIT(13))) +#define TARGET_IO_PORTD BIT(2) #define USABLE_TIMER_CHANNEL_COUNT 12 #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)) From 14a126c52e05800603bafa05652bb3971c32234f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 27 Jan 2017 17:12:01 +0000 Subject: [PATCH 53/55] Moved gps_conversion to common/ directory --- Makefile | 2 +- src/main/{flight => common}/gps_conversion.c | 0 src/main/{flight => common}/gps_conversion.h | 0 src/main/flight/navigation.c | 16 ++++++++-------- src/main/io/gps.c | 20 ++++++++++---------- 5 files changed, 19 insertions(+), 19 deletions(-) rename src/main/{flight => common}/gps_conversion.c (100%) rename src/main/{flight => common}/gps_conversion.h (100%) diff --git a/Makefile b/Makefile index e5bc1e4ad..f64a88aea 100644 --- a/Makefile +++ b/Makefile @@ -650,6 +650,7 @@ HIGHEND_SRC = \ cms/cms_menu_osd.c \ cms/cms_menu_vtx.c \ common/colorconversion.c \ + common/gps_conversion.c \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ drivers/serial_escserial.c \ @@ -657,7 +658,6 @@ HIGHEND_SRC = \ drivers/sonar_hcsr04.c \ drivers/vtx_common.c \ flight/navigation.c \ - flight/gps_conversion.c \ io/dashboard.c \ io/displayport_max7456.c \ io/displayport_msp.c \ diff --git a/src/main/flight/gps_conversion.c b/src/main/common/gps_conversion.c similarity index 100% rename from src/main/flight/gps_conversion.c rename to src/main/common/gps_conversion.c diff --git a/src/main/flight/gps_conversion.h b/src/main/common/gps_conversion.h similarity index 100% rename from src/main/flight/gps_conversion.h rename to src/main/common/gps_conversion.h diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index dffd45689..fc1c4a144 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -27,6 +27,7 @@ #include "build/debug.h" #include "common/axis.h" +#include "common/gps_conversion.h" #include "common/maths.h" #include "common/time.h" @@ -36,21 +37,20 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "sensors/sensors.h" -#include "sensors/boardalignment.h" -#include "sensors/acceleration.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/pid.h" #include "io/beeper.h" #include "io/serial.h" #include "io/gps.h" -#include "flight/pid.h" -#include "flight/navigation.h" -#include "flight/gps_conversion.h" -#include "flight/imu.h" - #include "rx/rx.h" +#include "sensors/acceleration.h" +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + extern int16_t magHold; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index c4925d8f4..9cae15f26 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -29,27 +29,27 @@ #include "build/build_config.h" #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/gps_conversion.h" +#include "common/maths.h" #include "common/utils.h" -#include "drivers/system.h" +#include "config/feature.h" + #include "drivers/light_led.h" +#include "drivers/system.h" -#include "sensors/sensors.h" - -#include "io/serial.h" #include "io/dashboard.h" #include "io/gps.h" - -#include "flight/gps_conversion.h" -#include "flight/pid.h" -#include "flight/navigation.h" +#include "io/serial.h" #include "fc/config.h" #include "fc/runtime_config.h" -#include "config/feature.h" +#include "flight/navigation.h" +#include "flight/pid.h" + +#include "sensors/sensors.h" #define LOG_ERROR '?' #define LOG_IGNORED '!' From 431e0e23dd0be7b70b6950a70116815518c8bcb6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 28 Jan 2017 07:37:15 +0000 Subject: [PATCH 54/55] Fixup test code --- src/test/Makefile | 16 ++++++++-------- src/test/unit/telemetry_crsf_unittest.cc | 14 +++++++------- src/test/unit/telemetry_hott_unittest.cc | 20 ++++++++++---------- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 9f1e83940..26b95987c 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -256,24 +256,24 @@ $(OBJECT_DIR)/altitude_hold_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ -$(OBJECT_DIR)/flight/gps_conversion.o : \ - $(USER_DIR)/flight/gps_conversion.c \ - $(USER_DIR)/flight/gps_conversion.h \ +$(OBJECT_DIR)/common/gps_conversion.o : \ + $(USER_DIR)/common/gps_conversion.c \ + $(USER_DIR)/common/gps_conversion.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) - $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@ + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/gps_conversion.c -o $@ $(OBJECT_DIR)/gps_conversion_unittest.o : \ $(TEST_DIR)/gps_conversion_unittest.cc \ - $(USER_DIR)/flight/gps_conversion.h \ + $(USER_DIR)/common/gps_conversion.h \ $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@ $(OBJECT_DIR)/gps_conversion_unittest : \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/gps_conversion_unittest.o \ $(OBJECT_DIR)/gtest_main.a @@ -359,7 +359,7 @@ $(OBJECT_DIR)/telemetry_hott_unittest.o : \ $(OBJECT_DIR)/telemetry_hott_unittest : \ $(OBJECT_DIR)/telemetry/hott.o \ $(OBJECT_DIR)/telemetry_hott_unittest.o \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/gtest_main.a $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ @@ -537,7 +537,7 @@ $(OBJECT_DIR)/telemetry_crsf_unittest : \ $(OBJECT_DIR)/telemetry_crsf_unittest.o \ $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/common/streambuf.o \ - $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/common/gps_conversion.o \ $(OBJECT_DIR)/fc/runtime_config.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index bb1a526d7..cfe99b794 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -28,30 +28,30 @@ extern "C" { #include "common/axis.h" #include "common/filter.h" + #include "common/gps_conversion.h" #include "common/maths.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" - #include "drivers/system.h" #include "drivers/serial.h" + #include "drivers/system.h" #include "fc/runtime_config.h" + #include "flight/pid.h" + #include "flight/imu.h" + #include "io/gps.h" #include "io/serial.h" #include "rx/crsf.h" - #include "sensors/sensors.h" #include "sensors/battery.h" + #include "sensors/sensors.h" - #include "telemetry/telemetry.h" #include "telemetry/crsf.h" - - #include "flight/pid.h" - #include "flight/imu.h" - #include "flight/gps_conversion.h" + #include "telemetry/telemetry.h" bool airMode; uint16_t vbat; diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index ff984b097..bc0d60ad0 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -27,24 +27,24 @@ extern "C" { #include "build/debug.h" #include "common/axis.h" + #include "common/gps_conversion.h" - #include "drivers/system.h" #include "drivers/serial.h" + #include "drivers/system.h" - #include "sensors/sensors.h" - #include "sensors/battery.h" - #include "sensors/barometer.h" + #include "fc/runtime_config.h" + + #include "flight/pid.h" - #include "io/serial.h" #include "io/gps.h" + #include "io/serial.h" + + #include "sensors/barometer.h" + #include "sensors/battery.h" + #include "sensors/sensors.h" #include "telemetry/telemetry.h" #include "telemetry/hott.h" - - #include "flight/pid.h" - #include "flight/gps_conversion.h" - - #include "fc/runtime_config.h" } #include "unittest_macros.h" From 4868b60b5b129cacbaf547254214fb00386393a6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 28 Jan 2017 07:30:32 +0000 Subject: [PATCH 55/55] Put #includes into alphabetical order --- src/main/fc/cli.c | 2 +- src/main/fc/fc_msp.c | 60 ++++++++++++++++++++++---------------------- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 4782d812e..ca7096211 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -45,8 +45,8 @@ uint8_t cliMode = 0; #include "common/typeconversion.h" #include "config/config_eeprom.h" -#include "config/config_profile.h" #include "config/config_master.h" +#include "config/config_profile.h" #include "config/feature.h" #include "config/parameter_group.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6de708dc6..45587418d 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -34,20 +34,25 @@ #include "common/maths.h" #include "common/streambuf.h" -#include "drivers/system.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + #include "drivers/accgyro.h" -#include "drivers/compass.h" -#include "drivers/serial.h" #include "drivers/bus_i2c.h" -#include "drivers/io.h" +#include "drivers/compass.h" #include "drivers/flash.h" -#include "drivers/sdcard.h" -#include "drivers/vcd.h" +#include "drivers/io.h" #include "drivers/max7456.h" -#include "drivers/vtx_soft_spi_rtc6705.h" #include "drivers/pwm_output.h" +#include "drivers/sdcard.h" +#include "drivers/serial.h" #include "drivers/serial_escserial.h" +#include "drivers/system.h" +#include "drivers/vcd.h" #include "drivers/vtx_common.h" +#include "drivers/vtx_soft_spi_rtc6705.h" #include "fc/config.h" #include "fc/fc_core.h" @@ -55,17 +60,25 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/navigation.h" +#include "flight/pid.h" +#include "flight/servos.h" + +#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" -#include "io/motors.h" -#include "io/servos.h" +#include "io/flashfs.h" #include "io/gps.h" #include "io/gimbal.h" -#include "io/serial.h" #include "io/ledstrip.h" -#include "io/flashfs.h" -#include "io/transponder_ir.h" -#include "io/asyncfatfs/asyncfatfs.h" +#include "io/motors.h" +#include "io/serial.h" #include "io/serial_4way.h" +#include "io/servos.h" +#include "io/transponder_ir.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -76,30 +89,17 @@ #include "scheduler/scheduler.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/sonar.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" #include "sensors/compass.h" #include "sensors/gyro.h" +#include "sensors/sensors.h" +#include "sensors/sonar.h" #include "telemetry/telemetry.h" -#include "flight/mixer.h" -#include "flight/servos.h" -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" -#include "flight/altitudehold.h" - -#include "config/config_eeprom.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif