From acbff49a1d964eec1f4a450373250895ebdb7fbc Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 21 Dec 2016 15:27:31 +0000 Subject: [PATCH 01/97] 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 df316179fc..f57f4f1c98 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 c932bd73131987192abb1259ca481d1e28a55c10 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Tue, 27 Dec 2016 13:25:37 +0300 Subject: [PATCH 02/97] 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 2c76289888..f0ce496c67 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -82,7 +82,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); @@ -90,7 +90,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); @@ -256,7 +257,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; } @@ -271,9 +273,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); } motors[motorIndex].enabled = true; } @@ -335,7 +337,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 a6ad7f38b3..c864889874 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -118,7 +118,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 7a624c3632..6a4f3260d4 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 55a48bcbaf..0f4b80f453 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 30ccd983e2..58e4f37bfa 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 218772e864..d8b5ad9809 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 88ea65d1fd..c80b6db8b7 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1143,6 +1143,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 : @@ -1479,7 +1480,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)) { @@ -1490,6 +1491,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 8486d507d2..82517b2e9d 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 b8ee92c9ea07bd9bf70a87b66a14f27a6bf1fa7b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 24 Jan 2017 22:38:36 +0000 Subject: [PATCH 03/97] Reviewed and revised compiler speed optimisations --- Makefile | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/Makefile b/Makefile index 0381e7412e..3dc8169cf1 100644 --- a/Makefile +++ b/Makefile @@ -707,26 +707,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/mw.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 \ @@ -747,25 +741,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 \ @@ -951,7 +932,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 deb2052d8e91d12f7e2fafaa83837087e2db9963 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 17:08:51 +0000 Subject: [PATCH 04/97] Updated version, MSP version and EEPROM_CONF_VERSION for 3.2 --- src/main/build/version.h | 2 +- src/main/config/config_eeprom.h | 2 +- src/main/msp/msp_protocol.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index 59a418b265..4770ad98e9 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -17,7 +17,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_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 diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 8d73a31967..645689ffd0 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/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 63d2b70bdb..d2c368f28a 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 73308e82465d87ff899761045af17057fd7b30b9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:16:05 +0000 Subject: [PATCH 05/97] 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 b452afc774..93405ba687 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 d24e4e01a398881807f942936a47be4bb203cf6a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:49:32 +0000 Subject: [PATCH 06/97] Changed back to using -Ofast optimisation --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 3dc8169cf1..ca30b79e23 100644 --- a/Makefile +++ b/Makefile @@ -932,7 +932,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 733e515610790efb877b3ba76b2a90082c0a65ba Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 26 Jan 2017 00:51:51 +1300 Subject: [PATCH 07/97] 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 22b28d1f63..a4cebaef36 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 a6db3d0210..1a1a9ac57c 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 b18c29cb61..8e6341e90b 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 fd475d50af..928acdc2b7 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 bdbd7b1a8aad70b2493e4094af8e8685a9c3318c Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 17 Jan 2017 01:04:38 +0100 Subject: [PATCH 08/97] 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 7e617b03ac..e43505d26c 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 c7c521c666..e0899cc85a 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 749a0d40c3..34da6e97fa 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 ebce3d984709ed5439b0a7d3595e0615042ea33a Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 17 Jan 2017 01:06:20 +0100 Subject: [PATCH 09/97] 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 c4a4599e8a..6f4dd83037 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" @@ -1187,6 +1188,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; } @@ -1634,15 +1664,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 2070246314ecf3fbde487af6ccbe1346122ef05e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 26 Jan 2017 08:56:22 +1300 Subject: [PATCH 10/97] 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 5072324926..df472a95ec 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 810716a57d..4ad98115d6 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 4603091ed4..959c6e9d8d 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 44004b0b0b9438948effd42ad406cd8f9c7c34ce Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 20 Dec 2016 22:42:58 +0000 Subject: [PATCH 11/97] 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 ca30b79e23..decd865b2e 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 5c32f18183..18cbfa8e9b 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 645689ffd0..a6aafc3ca7 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 a0ffa53905..d1f2c513fd 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 0000000000..7ca0eda77a --- /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 0000000000..cd48ea9f72 --- /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 0000000000..a62356fbea --- /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 22b28d1f63..59872dc309 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 @@ -1122,12 +1123,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 0fe4f9e616..4a77447dfa 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 c4a4599e8a..c2b920deca 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -104,7 +104,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 2cc81c9fd5..eaf65e7b9a 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 106542008f6713d8f5682e9714e6a69be691d459 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Dec 2016 13:16:43 +0000 Subject: [PATCH 12/97] 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 7ca0eda77a..5d13e12475 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 6ee5cf1d5d914b851f9c1c83b5782b2afc077621 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Dec 2016 17:15:16 +0000 Subject: [PATCH 13/97] 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 18cbfa8e9b..2850700c54 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 c42ef32f0e..fbef2e84d5 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 59872dc309..fccd15b8fa 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -547,7 +547,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; @@ -1132,10 +1132,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 4a77447dfa..62841f5fd3 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 6ef5f0579ac82d1b83491dc96332f37ab86b5835 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 2 Jan 2017 17:03:17 +0000 Subject: [PATCH 14/97] 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 cd48ea9f72..e63363f22d 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 87af44df54..5ac340c31e 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 d233d3852d..d44b854e79 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 @@ -808,8 +829,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) { @@ -928,6 +948,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; @@ -942,6 +980,7 @@ void *getValuePointer(const clivalue_t *value) return ptr; } +#endif static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig) { @@ -1003,7 +1042,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) { @@ -1050,13 +1089,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: @@ -3072,7 +3105,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); @@ -3103,7 +3136,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 @@ -3124,7 +3157,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)) { @@ -3540,6 +3573,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; @@ -3556,13 +3601,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 } @@ -3685,6 +3738,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) @@ -3811,13 +3868,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 SKIP_CLI_COMMAND_HELP if (cmdTable[i].description) { @@ -3846,7 +3901,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) @@ -3907,12 +3962,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 fccd15b8fa..1bbd9894bc 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -869,11 +869,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(); @@ -1166,7 +1169,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 62841f5fd3..6466507e61 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 0bca9e0c7acd5b10ff9d7424ab7fbdeef369bb50 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 10:26:25 +0000 Subject: [PATCH 15/97] 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 734933ca9c..2533b50158 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 d44b854e79..52f7545367 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) @@ -855,6 +844,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)) { @@ -949,23 +948,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; @@ -1138,9 +1235,9 @@ static void cliShowArgumentRangeError(char *name, int min, int max) cliPrintf("%s must be 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++; } @@ -1148,14 +1245,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) { @@ -1174,10 +1269,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++) { @@ -1216,7 +1311,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]; @@ -1224,10 +1319,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)) { @@ -1334,7 +1429,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); @@ -1408,13 +1503,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)); @@ -1423,9 +1514,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; @@ -1439,7 +1530,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; @@ -1488,7 +1579,6 @@ static void cliSerial(char *cmdline) } memcpy(currentConfig, &portConfig, sizeof(portConfig)); - } #ifndef SKIP_SERIAL_PASSTHROUGH @@ -1607,7 +1697,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); @@ -1711,7 +1801,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); @@ -1799,7 +1889,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); @@ -1861,7 +1951,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); @@ -1899,7 +1989,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); @@ -2177,10 +2267,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); @@ -2191,7 +2279,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++) { @@ -2209,7 +2297,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) { @@ -2251,7 +2339,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, " "); @@ -2425,7 +2513,7 @@ static void cliFlashRead(char *cmdline) uint8_t buffer[32]; - char *nextArg = strchr(cmdline, ' '); + const char *nextArg = strchr(cmdline, ' '); if (!nextArg) { cliShowParseError(); @@ -2494,7 +2582,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); @@ -3065,7 +3153,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(""); } @@ -3699,7 +3793,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); @@ -3839,7 +3933,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 fc296533cd01a4765d42f9a84c3f79327a07899b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 11:34:20 +0000 Subject: [PATCH 16/97] 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 b9cc396a3a..315faf2fe5 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 df45a1829c..a48e98e055 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 e4c860d4ddd8497a7ae6f99bc2dee18ff4b5acf0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 12:04:41 +0000 Subject: [PATCH 17/97] 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 e63363f22d..c4b6f6af03 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 107fc11939c9c873ae890d583ad2153cd4b0eead Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Dec 2016 19:31:02 +0100 Subject: [PATCH 18/97] 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 959c6e9d8d..f2436d5534 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 afc71be60ce8eae404b736f1d1b36eac4eb05f64 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Fri, 30 Dec 2016 12:57:34 +0100 Subject: [PATCH 19/97] 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 f2436d5534..3708e911ae 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 7f8f7deab97bb28c211e35663ce76c2876bf116d Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 26 Jan 2017 10:05:17 +1300 Subject: [PATCH 20/97] 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 3708e911ae..0508f77e4f 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 f839e658e756ab3496aa8d821a59f50bf5ba451a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 21:06:01 +0000 Subject: [PATCH 21/97] 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 2850700c54..295cd77b97 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 a6aafc3ca7..62c8c6181a 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 1bbd9894bc..63f5dc6a65 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 @@ -1159,6 +1160,12 @@ void writeEEPROM(void) resumeRxSignal(); } +void resetEEPROM(void) +{ + resetConfigs(); + writeEEPROM(); +} + void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { @@ -1167,12 +1174,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 6466507e61..7a85b85384 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 6a4e6e58859aab85b9b2f9cd583c65bea563a12d Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 26 Jan 2017 11:33:29 +0900 Subject: [PATCH 22/97] 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 6d4289da5a..e97ff6468b 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 65e0d9990bf06ebd3232ed0a3c705e650327fb12 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 27 Jan 2017 17:12:01 +0000 Subject: [PATCH 23/97] 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 decd865b2e..4350bba43a 100644 --- a/Makefile +++ b/Makefile @@ -649,6 +649,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 \ @@ -656,7 +657,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 dffd456893..fc1c4a144d 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 c4925d8f4e..9cae15f265 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 bfbacf8b444b34ab1d0831b316162f50d790a299 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 28 Jan 2017 07:37:15 +0000 Subject: [PATCH 24/97] 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 9f1e83940d..26b95987ce 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 bb1a526d7e..cfe99b7943 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 ff984b097b..bc0d60ad01 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 f3b38f4f09b2c8bb6bb8dc3713e6ba39eb83d9db Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 28 Jan 2017 07:30:32 +0000 Subject: [PATCH 25/97] 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 ef80bd96d1..8e063dbed3 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 e28f09b081..2923c7aac6 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 From 6140a9bb573141c01cecdea86ee9b458567c8087 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 29 Jan 2017 11:03:48 +1300 Subject: [PATCH 26/97] Cleaned up SPRACINGF3 (and TINYBEEF3 variant) targets. --- src/main/target/SPRACINGF3MINI/target.h | 88 ++++++++++++------------- 1 file changed, 43 insertions(+), 45 deletions(-) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index ebff516b0e..3334b77397 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -28,9 +28,6 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -// early prototype had slightly different pin mappings. -//#define SPRACINGF3MINI_MKII_REVA - #define LED0 PB3 #endif @@ -39,36 +36,35 @@ #define USE_EXTI #define MPU_INT_EXTI PC13 -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define ACC -#define BARO -#define USE_BARO_BMP280 - #ifdef TINYBEEF3 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT + #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW270_DEG #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW270_DEG - -#define MAG_AK8963_ALIGN CW90_DEG_FLIP #else -//#define USE_FAKE_GYRO +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + #define USE_GYRO_MPU6500 #define GYRO_MPU6500_ALIGN CW180_DEG -//#define USE_FAKE_ACC #define USE_ACC_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG +#define BARO +#define USE_BARO_BMP280 + #define MAG #define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8975 @@ -80,24 +76,20 @@ //#define SONAR_ECHO_PIN PB1 //#define SONAR_TRIGGER_PIN PB0 +#define BRUSHED_ESC_AUTODETECT + #define USB_IO -#ifndef TINYBEEF3 -#define USB_CABLE_DETECTION - -#define USB_DETECT_PIN PB5 -#endif - #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -107,21 +99,26 @@ #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#ifdef TINYBEEF3 +#define SERIAL_PORT_COUNT 4 +#else +#define USB_CABLE_DETECTION +#define USB_DETECT_PIN PB5 + +#define USE_SOFTSERIAL1 #define SOFTSERIAL_1_TIMER TIM2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 #define SONAR_SOFTSERIAL1_EXCLUSIVE -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define SERIAL_PORT_COUNT 5 +#endif #define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 #ifdef TINYBEEF3 #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -131,9 +128,18 @@ #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 -#define MPU6500_CS_PIN PB9 +#define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 -#endif +#else +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define USE_SDCARD #define USE_SDCARD_SPI2 @@ -156,6 +162,9 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#endif + #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 @@ -169,16 +178,11 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#ifdef TINYBEEF3 -#define BRUSHED_ESC_AUTODETECT -#else -#define DEFAULT_FEATURES FEATURE_BLACKBOX -#endif #ifndef TINYBEEF3 +#define DEFAULT_FEATURES FEATURE_BLACKBOX + #define BUTTONS #define BUTTON_A_PIN PB1 #define BUTTON_B_PIN PB0 @@ -187,12 +191,6 @@ #define BINDPLUG_PIN PB0 #endif -#define SPEKTRUM_BIND -// USART3, -#define BIND_PIN PB11 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) From 8f3ef2676fd8b5244aabe994c490a0b8bc17d942 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 21 Dec 2016 15:27:31 +0000 Subject: [PATCH 27/97] 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 26cce45584..5899c69f87 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 28/97] 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 8f99c16adc..eb71818010 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 7102e7e351..97c5f5e662 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 5ae299dcf0..9e29cedaa5 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 03156953a0..95d53b3d5d 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 b898493250..848cb88ea6 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 bb242e96ce..87ca6b2e1f 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 404a895468..8e74b28931 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 8486d507d2..82517b2e9d 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 29/97] Reviewed and revised compiler speed optimisations --- Makefile | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/Makefile b/Makefile index 56c02912f9..012a03b231 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 30/97] 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 3df77b8cf7..4770ad98e9 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 63d2b70bdb..d2c368f28a 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 31/97] 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 b452afc774..93405ba687 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 32/97] Changed back to using -Ofast optimisation --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 012a03b231..5e9cbb6303 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 33/97] 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 7e617b03ac..e43505d26c 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 c7c521c666..e0899cc85a 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 749a0d40c3..34da6e97fa 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 34/97] 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 8e74b28931..6b3b86a87e 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 35/97] 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 5072324926..df472a95ec 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 810716a57d..4ad98115d6 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 4603091ed4..959c6e9d8d 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 36/97] 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 5e9cbb6303..e5bc1e4ad3 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 5c32f18183..18cbfa8e9b 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 645689ffd0..a6aafc3ca7 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 a0ffa53905..d1f2c513fd 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 0000000000..7ca0eda77a --- /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 0000000000..cd48ea9f72 --- /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 0000000000..a62356fbea --- /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 e4d2b69be3..8ceecd6225 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 0fe4f9e616..4a77447dfa 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 6b3b86a87e..6de708dc6d 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 2cc81c9fd5..eaf65e7b9a 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 37/97] 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 7ca0eda77a..5d13e12475 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 38/97] 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 18cbfa8e9b..2850700c54 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 c42ef32f0e..fbef2e84d5 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 8ceecd6225..e65ef76b22 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 4a77447dfa..62841f5fd3 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 39/97] 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 cd48ea9f72..e63363f22d 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 87af44df54..5ac340c31e 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 87ca6b2e1f..abb8dc147e 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 e65ef76b22..e66c6e1395 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 62841f5fd3..6466507e61 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 40/97] 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 734933ca9c..2533b50158 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 abb8dc147e..4782d812e8 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 41/97] 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 b9cc396a3a..315faf2fe5 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 df45a1829c..a48e98e055 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 42/97] 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 e63363f22d..c4b6f6af03 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 43/97] 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 959c6e9d8d..f2436d5534 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 44/97] 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 f2436d5534..3708e911ae 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 45/97] 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 3708e911ae..0508f77e4f 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 46/97] 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 2850700c54..295cd77b97 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 a6aafc3ca7..62c8c6181a 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 e66c6e1395..8ee0519c81 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 6466507e61..7a85b85384 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 47/97] 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 6d4289da5a..e97ff6468b 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 48/97] 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 e5bc1e4ad3..f64a88aea7 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 dffd456893..fc1c4a144d 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 c4925d8f4e..9cae15f265 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 49/97] 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 9f1e83940d..26b95987ce 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 bb1a526d7e..cfe99b7943 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 ff984b097b..bc0d60ad01 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 50/97] 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 4782d812e8..ca70962113 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 6de708dc6d..45587418d7 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 From 4765ff410948fe7ef327e8a67d359fb12d3a3175 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 31 Jan 2017 14:00:25 +1300 Subject: [PATCH 51/97] Clarified resource remapping output and fixed output in case of double mapping. --- src/main/fc/cli.c | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ca70962113..d6507da396 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3501,9 +3501,18 @@ static void printResource(uint8_t dumpMask, const master_t *defaultConfig) } } +static void printResourceOwner(uint8_t owner, uint8_t index) +{ + cliPrintf("%s", ownerNames[resourceTable[owner].owner]); + + if (resourceTable[owner].maxIndex > 0) { + cliPrintf(" %d", RESOURCE_INDEX(index)); + } +} + static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { - const char * format = "\r\n* NOTE * %c%d moved from %s"; + const char * format = "\r\nNOTE: %c%02d already assigned to "; 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) { @@ -3516,14 +3525,17 @@ static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) cleared = true; } - 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)); - } + cliPrintf(format, DEFIO_TAG_GPIOID(tag) + 'A', DEFIO_TAG_PIN(tag)); + + printResourceOwner(r, i); + if (cleared) { - cliPrintf(". Cleared."); + cliPrintf(". "); + printResourceOwner(r, i); + cliPrintf(" disabled"); } - cliPrint("\r\n"); + + cliPrint(".\r\n"); } } } From 61d72b8738d55610b951db909f3a0a73bbadf67e Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 29 Jan 2017 18:07:18 +0100 Subject: [PATCH 52/97] Renamed MPU function pointers, align with iNav --- src/main/drivers/accgyro_mpu.c | 47 ++++++++++++------------- src/main/drivers/accgyro_mpu.h | 22 ++++++------ src/main/drivers/accgyro_mpu3050.c | 12 +++---- src/main/drivers/accgyro_mpu6050.c | 16 ++++----- src/main/drivers/accgyro_mpu6500.c | 22 ++++++------ src/main/drivers/accgyro_spi_icm20689.c | 22 ++++++------ src/main/drivers/system_stm32f4xx.c | 8 ++--- src/main/drivers/system_stm32f7xx.c | 8 ++--- src/main/sensors/gyro.c | 2 +- 9 files changed, 79 insertions(+), 80 deletions(-) diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 93405ba687..9e7c3ea98f 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -48,7 +48,7 @@ #include "accgyro_mpu.h" -mpuResetFuncPtr mpuReset; +mpuResetFnPtr mpuResetFn; #ifndef MPU_I2C_INSTANCE #define MPU_I2C_INSTANCE I2C_DEVICE @@ -75,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // determine product ID and accel revision - ack = gyro->mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer); + ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); if (revision) { /* Congrats, these parts are better. */ @@ -89,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { - ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); + ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId); revision = productId & 0x0F; if (!revision) { failureMode(FAILURE_ACC_INCOMPATIBLE); @@ -176,7 +176,7 @@ bool mpuAccRead(accDev_t *acc) { uint8_t data[6]; - bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data); + bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data); if (!ack) { return false; } @@ -199,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro) { uint8_t data[6]; - const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data); + const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data); if (!ack) { return false; } @@ -230,8 +230,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6000SpiDetect()) { gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6000ReadRegister; - gyro->mpuConfiguration.write = mpu6000WriteRegister; + gyro->mpuConfiguration.readFn = mpu6000ReadRegister; + gyro->mpuConfiguration.writeFn = mpu6000WriteRegister; return true; } #endif @@ -241,8 +241,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6500Sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6500ReadRegister; - gyro->mpuConfiguration.write = mpu6500WriteRegister; + gyro->mpuConfiguration.readFn = mpu6500ReadRegister; + gyro->mpuConfiguration.writeFn = mpu6500WriteRegister; return true; } #endif @@ -251,11 +251,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) 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; + gyro->mpuConfiguration.readFn = mpu9250ReadRegister; + gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister; + gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister; + gyro->mpuConfiguration.writeFn = mpu9250WriteRegister; + gyro->mpuConfiguration.resetFn = mpu9250ResetGyro; return true; } #endif @@ -264,8 +264,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (icm20689SpiDetect()) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = icm20689ReadRegister; - gyro->mpuConfiguration.write = icm20689WriteRegister; + gyro->mpuConfiguration.readFn = icm20689ReadRegister; + gyro->mpuConfiguration.writeFn = icm20689WriteRegister; return true; } #endif @@ -277,22 +277,20 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) 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; + uint8_t sig = 0; + bool ack = false; #else - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); + uint8_t sig; + bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); #endif if (ack) { - gyro->mpuConfiguration.read = mpuReadRegisterI2C; - gyro->mpuConfiguration.write = mpuWriteRegisterI2C; + gyro->mpuConfiguration.readFn = mpuReadRegisterI2C; + gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C; } else { #ifdef USE_SPI bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); @@ -305,6 +303,7 @@ mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; // If an MPU3050 is connected sig will contain 0. + uint8_t inquiryResult; ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 8ba01540a3..8a44af0464 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -124,18 +124,18 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data); -typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); -typedef void(*mpuResetFuncPtr)(void); +typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data); +typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data); +typedef void(*mpuResetFnPtr)(void); -extern mpuResetFuncPtr mpuReset; +extern mpuResetFnPtr mpuResetFn; typedef struct mpuConfiguration_s { - mpuReadRegisterFunc read; - mpuWriteRegisterFunc write; - mpuReadRegisterFunc slowread; - mpuWriteRegisterFunc verifywrite; - mpuResetFuncPtr reset; + mpuReadRegisterFnPtr readFn; + mpuWriteRegisterFnPtr writeFn; + mpuReadRegisterFnPtr slowreadFn; + mpuWriteRegisterFnPtr verifywriteFn; + mpuResetFnPtr resetFn; uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each } mpuConfiguration_t; @@ -178,7 +178,7 @@ typedef enum { ICM_20689_SPI, ICM_20608_SPI, ICM_20602_SPI -} detectedMPUSensor_e; +} mpuSensor_e; typedef enum { MPU_HALF_RESOLUTION, @@ -186,7 +186,7 @@ typedef enum { } mpu6050Resolution_e; typedef struct mpuDetectionResult_s { - detectedMPUSensor_e sensor; + mpuSensor_e sensor; mpu6050Resolution_e resolution; } mpuDetectionResult_t; diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index 15728d9e12..374604b7c1 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -53,21 +53,21 @@ static void mpu3050Init(gyroDev_t *gyro) delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe - ack = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0); + ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0); if (!ack) failureMode(FAILURE_ACC_INIT); - gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); - gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0); - gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET); - gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); + gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0); + gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET); + gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); } static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) { UNUSED(gyro); uint8_t buf[2]; - if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) { + if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) { return false; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 631f7ec805..0a1fe6f336 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. // Accel scale 8g (4096 LSB/g) - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); + gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); #endif } diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index f9a46fbfc1..30d7caa0fb 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro) { mpuGyroInit(gyro); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); - gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); + gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); + gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration #ifdef USE_MPU9250_MAG - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable + gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif delay(15); } diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 29e22d5743..8d0af213df 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -130,32 +130,32 @@ void icm20689GyroInit(gyroDev_t *gyro) spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); delay(100); - gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03); + gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); -// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); +// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); // delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); delay(15); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); + gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); delay(15); - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); delay(15); - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops delay(100); // Data ready interrupt configuration -// gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN - gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN +// gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #ifdef USE_MPU_DATA_READY_SIGNAL - gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable + gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 55d570eecb..0216d45427 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -31,8 +31,8 @@ void SetSysClock(void); void systemReset(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } __disable_irq(); @@ -41,8 +41,8 @@ void systemReset(void) void systemResetToBootloader(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index 3254c475d6..0099833674 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -33,8 +33,8 @@ void SystemClock_Config(void); void systemReset(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } __disable_irq(); @@ -43,8 +43,8 @@ void systemReset(void) void systemResetToBootloader(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9732a96859..d0b15ece6b 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -238,7 +238,7 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); mpuDetect(&gyro.dev); - mpuReset = gyro.dev.mpuConfiguration.reset; + mpuResetFn = gyro.dev.mpuConfiguration.resetFn; #endif if (!gyroDetect(&gyro.dev)) { From 8c4914b3307291ed5e43e7e9af457ef4a0e52b77 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 21 Dec 2016 15:27:31 +0000 Subject: [PATCH 53/97] 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 26cce45584..5899c69f87 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 0e75b4131ee9e6def07681c37a3fee03ca90552f Mon Sep 17 00:00:00 2001 From: DieHertz Date: Tue, 27 Dec 2016 13:25:37 +0300 Subject: [PATCH 54/97] 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 8f99c16adc..eb71818010 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 7102e7e351..97c5f5e662 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 5ae299dcf0..9e29cedaa5 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 03156953a0..95d53b3d5d 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 b898493250..848cb88ea6 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 700711f1ab..7155658ffd 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 775b3f190e..6b3b86a87e 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1145,6 +1145,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 : @@ -1510,7 +1511,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)) { @@ -1521,6 +1522,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 8486d507d2..82517b2e9d 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 34fcf7666a415d05a481ce4660b07a720086f570 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 24 Jan 2017 22:38:36 +0000 Subject: [PATCH 55/97] Reviewed and revised compiler speed optimisations --- Makefile | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/Makefile b/Makefile index 56c02912f9..012a03b231 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 18a65575e7f2a3443f15833efc90b5ef19abef7c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 17:08:51 +0000 Subject: [PATCH 56/97] Updated version, MSP version and EEPROM_CONF_VERSION for 3.2 --- src/main/msp/msp_protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 63d2b70bdb..d2c368f28a 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 5852e999983ac45231df08c4883c951ee7ac5852 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:16:05 +0000 Subject: [PATCH 57/97] 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 b452afc774..93405ba687 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 c190c55ce3cf14eb485178fa8a2e89bc42d5bb05 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:49:32 +0000 Subject: [PATCH 58/97] Changed back to using -Ofast optimisation --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 012a03b231..5e9cbb6303 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 cbd036a1876f21ee92676485b9c66765ff952392 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 26 Jan 2017 08:56:22 +1300 Subject: [PATCH 59/97] 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 5072324926..df472a95ec 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 810716a57d..4ad98115d6 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 4603091ed4..959c6e9d8d 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 974037c15042b94cf5c74ede836c344458679a6c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 20 Dec 2016 22:42:58 +0000 Subject: [PATCH 60/97] 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 5e9cbb6303..e5bc1e4ad3 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 5c32f18183..18cbfa8e9b 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 645689ffd0..a6aafc3ca7 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 a0ffa53905..d1f2c513fd 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 0000000000..7ca0eda77a --- /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 0000000000..cd48ea9f72 --- /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 0000000000..a62356fbea --- /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 e4d2b69be3..8ceecd6225 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 0fe4f9e616..4a77447dfa 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 6b3b86a87e..6de708dc6d 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 2cc81c9fd5..eaf65e7b9a 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 029015afd8c0800d6cce599f1880c08665aa02b1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Dec 2016 13:16:43 +0000 Subject: [PATCH 61/97] 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 7ca0eda77a..5d13e12475 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 2aa89cf791d7f54a1d1a68428400163767d1de52 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 29 Dec 2016 17:15:16 +0000 Subject: [PATCH 62/97] 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 18cbfa8e9b..2850700c54 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 c42ef32f0e..fbef2e84d5 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 8ceecd6225..e65ef76b22 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 4a77447dfa..62841f5fd3 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 1c08319ef9165f520fdb5cbca7e8986417afddf0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 2 Jan 2017 17:03:17 +0000 Subject: [PATCH 63/97] 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 cd48ea9f72..e63363f22d 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 87af44df54..5ac340c31e 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 7155658ffd..c641ca7f35 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)) { @@ -3541,6 +3574,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; @@ -3557,13 +3602,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 } @@ -3686,6 +3739,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) @@ -3812,13 +3869,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) { @@ -3847,7 +3902,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) @@ -3908,12 +3963,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 e65ef76b22..e66c6e1395 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 62841f5fd3..6466507e61 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 cbc4629834c40653a3b01a36b10cfe5b9131f172 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 10:26:25 +0000 Subject: [PATCH 64/97] 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 734933ca9c..2533b50158 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 c641ca7f35..8c004b6851 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(""); } @@ -3700,7 +3794,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); @@ -3840,7 +3934,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 ec12c35555b5ccc0d2a5c450680865a6977913c7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 11:34:20 +0000 Subject: [PATCH 65/97] 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 b9cc396a3a..315faf2fe5 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 df45a1829c..a48e98e055 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 3dc03d7735e3fa13d8a181959b772394eec1bd52 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 12:04:41 +0000 Subject: [PATCH 66/97] 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 e63363f22d..c4b6f6af03 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 5b11326eb1694af7ac8010080ebb680b8a7a5a17 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Dec 2016 19:31:02 +0100 Subject: [PATCH 67/97] 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 959c6e9d8d..f2436d5534 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 2171d0115aa696361fee4d9d5cefe20c937ae6d0 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Fri, 30 Dec 2016 12:57:34 +0100 Subject: [PATCH 68/97] 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 f2436d5534..3708e911ae 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 deb5363873393f3c890f4a7c8a7a8004ebc6268e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 26 Jan 2017 10:05:17 +1300 Subject: [PATCH 69/97] 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 3708e911ae..0508f77e4f 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 8a26c3bcfb2c5f8639026822ba29be4466f8a4e2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 21:06:01 +0000 Subject: [PATCH 70/97] 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 2850700c54..295cd77b97 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 a6aafc3ca7..62c8c6181a 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 e66c6e1395..8ee0519c81 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 6466507e61..7a85b85384 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 3e85d1fe14e60e9e089620348dc72b428e5b5bbf Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 26 Jan 2017 11:33:29 +0900 Subject: [PATCH 71/97] 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 6d4289da5a..e97ff6468b 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 ca11e3de8e749cba9244e6864823f33e35fcfee0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 27 Jan 2017 17:12:01 +0000 Subject: [PATCH 72/97] 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 e5bc1e4ad3..f64a88aea7 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 dffd456893..fc1c4a144d 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 c4925d8f4e..9cae15f265 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 003d331291219057a09765611273d01e97506346 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 28 Jan 2017 07:37:15 +0000 Subject: [PATCH 73/97] 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 9f1e83940d..26b95987ce 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 bb1a526d7e..cfe99b7943 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 ff984b097b..bc0d60ad01 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 7e76dc80a45664d50a3d87804158682959f73194 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 28 Jan 2017 07:30:32 +0000 Subject: [PATCH 74/97] 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 8c004b6851..d6507da396 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 6de708dc6d..45587418d7 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 From 7521cd0fcb89318f8466770891851b3437a6d7c7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 24 Jan 2017 22:38:36 +0000 Subject: [PATCH 75/97] Reviewed and revised compiler speed optimisations --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index f64a88aea7..d295c94fdd 100644 --- a/Makefile +++ b/Makefile @@ -934,7 +934,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 c1e84c1bfd833784ac1b9d0e65b036e6d71f630e Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 17:08:51 +0000 Subject: [PATCH 76/97] Updated version, MSP version and EEPROM_CONF_VERSION for 3.2 --- src/main/build/version.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index 3df77b8cf7..4770ad98e9 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) From ae9fc8a58d9cb6e83bbbcbadfea06b66acd5cb41 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 25 Jan 2017 18:49:32 +0000 Subject: [PATCH 77/97] Changed back to using -Ofast optimisation --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index d295c94fdd..f64a88aea7 100644 --- a/Makefile +++ b/Makefile @@ -934,7 +934,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 c02ff315800223b6c1da47c0675c4f322788d305 Mon Sep 17 00:00:00 2001 From: Dan Nixon Date: Mon, 30 Jan 2017 12:16:21 +0000 Subject: [PATCH 78/97] Add blackbox state and free space to OSD stats --- src/main/io/osd.c | 53 +++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 51 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9f494ecb5f..206f1a1e5d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -31,6 +32,8 @@ #ifdef OSD +#include "blackbox/blackbox_io.h" + #include "build/debug.h" #include "build/version.h" @@ -50,9 +53,8 @@ #include "io/flashfs.h" #include "io/osd.h" - #include "io/vtx.h" - +#include "io/asyncfatfs/asyncfatfs.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -610,6 +612,47 @@ static void osdUpdateStats(void) stats.max_altitude = baro.BaroAlt; } +static void osdGetBlackboxStatusString(char * buff, uint8_t len) +{ + bool storageDeviceIsWorking = false; + uint32_t storageUsed = 0; + uint32_t storageTotal = 0; + + switch (blackboxConfig()->device) + { +#ifdef USE_SDCARD + case BLACKBOX_DEVICE_SDCARD: + storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY); + if (storageDeviceIsWorking) { + storageTotal = sdcard_getMetadata()->numBlocks / 2000; + storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000); + } + break; +#endif + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + storageDeviceIsWorking = flashfsIsReady(); + if (storageDeviceIsWorking) { + const flashGeometry_t *geometry = flashfsGetGeometry(); + storageTotal = geometry->totalSize / 1024; + storageUsed = flashfsGetOffset() / 1024; + } + break; +#endif + + default: + storageDeviceIsWorking = true; + } + + if (storageDeviceIsWorking) { + uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal; + snprintf(buff, len, "%d%%", storageUsedPercent); + } else { + snprintf(buff, len, "FAULT"); + } +} + static void osdShowStats(void) { uint8_t top = 2; @@ -650,6 +693,12 @@ static void osdShowStats(void) sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); displayWrite(osdDisplayPort, 22, top++, buff); + if (feature(FEATURE_BLACKBOX) && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) { + displayWrite(osdDisplayPort, 2, top, "BLACKBOX :"); + osdGetBlackboxStatusString(buff, 10); + displayWrite(osdDisplayPort, 22, top++, buff); + } + refreshTimeout = 60 * REFRESH_1S; } From 8c96d9e1ab80ec8e30ac64f9c4f89f987f685718 Mon Sep 17 00:00:00 2001 From: Martin Date: Tue, 31 Jan 2017 12:15:41 -0500 Subject: [PATCH 79/97] CLI: use strtok_r instead of strtok --- src/main/fc/cli.c | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d6507da396..e37d011b0f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1595,7 +1595,8 @@ static void cliSerialPassthrough(char *cmdline) int id = -1; uint32_t baud = 0; unsigned mode = 0; - char* tok = strtok(cmdline, " "); + char *saveptr; + char* tok = strtok_r(cmdline, " ", &saveptr); int index = 0; while (tok != NULL) { @@ -1614,7 +1615,7 @@ static void cliSerialPassthrough(char *cmdline) break; } index++; - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ", &saveptr); } serialPort_t *passThroughPort; @@ -2054,10 +2055,11 @@ static void cliModeColor(char *cmdline) enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; int argNo = 0; - char* ptr = strtok(cmdline, " "); + char *saveptr; + char* ptr = strtok_r(cmdline, " ", &saveptr); while (ptr && argNo < ARGS_COUNT) { args[argNo++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr != NULL || argNo != ARGS_COUNT) { @@ -2317,10 +2319,11 @@ static void cliServoMix(char *cmdline) return; } - ptr = strtok(ptr, " "); + char *saveptr; + ptr = strtok_r(ptr, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT - 1) { args[check++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr == NULL || check != ARGS_COUNT - 1) { @@ -2341,10 +2344,11 @@ static void cliServoMix(char *cmdline) cliServoMix("reverse"); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; - char *ptr = strtok(cmdline, " "); + char *saveptr; + ptr = strtok_r(cmdline, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT) { args[check++] = atoi(ptr); - ptr = strtok(NULL, " "); + ptr = strtok_r(NULL, " ", &saveptr); } if (ptr != NULL || check != ARGS_COUNT) { From 17f25d370deb0168f3edc7d52217ff7b13d3ee31 Mon Sep 17 00:00:00 2001 From: Martin Date: Tue, 31 Jan 2017 12:43:59 -0500 Subject: [PATCH 80/97] fix strtok_r --- 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 e37d011b0f..f6536a9eea 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2345,7 +2345,7 @@ static void cliServoMix(char *cmdline) } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; char *saveptr; - ptr = strtok_r(cmdline, " ", &saveptr); + char *ptr = strtok_r(cmdline, " ", &saveptr); while (ptr != NULL && check < ARGS_COUNT) { args[check++] = atoi(ptr); ptr = strtok_r(NULL, " ", &saveptr); From c55aced39ba20375bcea6c5fdb45cbac1548e77b Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 1 Feb 2017 08:28:44 +1100 Subject: [PATCH 81/97] Removed superfluous unnecessary comments. --- src/main/target/RCEXPLORERF3/target.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c index 5e910d8ff6..28bec2ccea 100644 --- a/src/main/target/RCEXPLORERF3/target.c +++ b/src/main/target/RCEXPLORERF3/target.c @@ -26,11 +26,11 @@ #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM3 - PA8 - DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1), // PWM2 - PA7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM4 - PB0 - DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM5 - PB1 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), // PWM6 - PPM - DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PWM6 - PPM + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), + DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), + DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, 0), + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), }; From 023309245a064bb0c44c17a6e552b250f9773095 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 1 Feb 2017 08:34:27 +1100 Subject: [PATCH 82/97] Update based on suggestion from @ledvinap --- src/main/fc/cli.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d6507da396..c8982b3408 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3443,7 +3443,7 @@ static void cliVersion(char *cmdline) #if defined(USE_RESOURCE_MGMT) -#define MAX_RESOURCE_INDEX(x) (x == 0 ? 1 : x) +#define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x)) typedef struct { const uint8_t owner; @@ -3512,6 +3512,10 @@ static void printResourceOwner(uint8_t owner, uint8_t index) static void resourceCheck(uint8_t resourceIndex, uint8_t index, ioTag_t tag) { + if (!tag) { + return; + } + const char * format = "\r\nNOTE: %c%02d already assigned to "; for (int r = 0; r < (int)ARRAYLEN(resourceTable); r++) { for (int i = 0; i < MAX_RESOURCE_INDEX(resourceTable[r].maxIndex); i++) { From 79d4b2146db77954ff072f7adeab0fa82221fd92 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 24 Jan 2017 19:42:29 +0000 Subject: [PATCH 83/97] Preparation for conversion to parameter groups --- src/main/blackbox/blackbox.c | 35 ++- src/main/blackbox/blackbox.h | 4 + src/main/blackbox/blackbox_io.c | 9 +- src/main/cms/cms.c | 15 +- src/main/cms/cms_menu_blackbox.c | 7 +- src/main/cms/cms_menu_imu.c | 3 +- src/main/cms/cms_menu_ledstrip.c | 7 +- src/main/cms/cms_menu_misc.c | 9 +- src/main/cms/cms_menu_osd.c | 7 +- src/main/cms/cms_menu_vtx.c | 11 +- src/main/config/config_master.h | 65 ++++- src/main/config/feature.c | 3 +- src/main/config/feature.h | 10 +- src/main/config/parameter_group.c | 3 +- src/main/config/parameter_group.h | 15 + src/main/config/parameter_group_ids.h | 4 + src/main/fc/cli.c | 312 +++++++++++++++++++-- src/main/fc/config.c | 18 +- src/main/fc/config.h | 6 + src/main/fc/fc_core.c | 21 +- src/main/fc/fc_init.c | 10 +- src/main/fc/fc_msp.c | 274 +++++++++--------- src/main/fc/fc_tasks.c | 19 +- src/main/fc/rc_controls.c | 17 +- src/main/fc/rc_controls.h | 22 +- src/main/fc/rc_curves.c | 2 +- src/main/fc/rc_curves.h | 2 +- src/main/flight/failsafe.c | 22 +- src/main/flight/failsafe.h | 8 +- src/main/flight/imu.c | 3 + src/main/flight/imu.h | 4 + src/main/flight/mixer.c | 70 +++-- src/main/flight/mixer.h | 27 +- src/main/flight/navigation.c | 3 + src/main/flight/pid.c | 3 + src/main/flight/pid.h | 4 + src/main/flight/servos.c | 30 +- src/main/flight/servos.h | 12 +- src/main/io/displayport_max7456.c | 3 +- src/main/io/gimbal.h | 4 + src/main/io/gps.c | 5 + src/main/io/gps.h | 4 + src/main/io/ledstrip.c | 12 +- src/main/io/ledstrip.h | 3 + src/main/io/motors.h | 12 +- src/main/io/osd.c | 11 +- src/main/io/osd.h | 4 + src/main/io/serial.c | 3 + src/main/io/serial.h | 3 + src/main/io/servos.h | 2 + src/main/io/vtx.c | 6 +- src/main/io/vtx_smartaudio.c | 16 +- src/main/rx/rx.c | 47 ++-- src/main/rx/rx.h | 9 + src/main/sensors/acceleration.c | 3 + src/main/sensors/acceleration.h | 3 + src/main/sensors/barometer.c | 15 +- src/main/sensors/barometer.h | 3 + src/main/sensors/battery.c | 64 +++-- src/main/sensors/battery.h | 9 +- src/main/sensors/boardalignment.c | 3 + src/main/sensors/boardalignment.h | 4 + src/main/sensors/compass.c | 3 + src/main/sensors/compass.h | 4 +- src/main/sensors/esc_sensor.c | 18 +- src/main/sensors/gyro.c | 41 +-- src/main/sensors/gyro.h | 3 + src/main/sensors/sonar.c | 2 + src/main/sensors/sonar.h | 3 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 30 +- src/main/target/common.h | 1 + src/main/telemetry/crsf.c | 4 +- src/main/telemetry/frsky.c | 36 ++- src/main/telemetry/frsky.h | 4 +- src/main/telemetry/hott.c | 16 +- src/main/telemetry/hott.h | 2 +- src/main/telemetry/ibus.c | 3 +- src/main/telemetry/ibus.h | 4 +- src/main/telemetry/ltm.c | 11 +- src/main/telemetry/ltm.h | 2 +- src/main/telemetry/mavlink.c | 44 +-- src/main/telemetry/smartport.c | 26 +- src/main/telemetry/smartport.h | 2 +- src/main/telemetry/telemetry.c | 23 +- src/main/telemetry/telemetry.h | 7 +- src/test/unit/cms_unittest.cc | 1 + src/test/unit/parameter_groups_unittest.cc | 2 +- src/test/unit/platform.h | 4 +- src/test/unit/telemetry_crsf_unittest.cc | 11 +- src/test/unit/telemetry_hott_unittest.cc | 6 + 90 files changed, 1150 insertions(+), 507 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49ceee0848..45b3bc1dbc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -24,6 +24,9 @@ #ifdef BLACKBOX +#include "blackbox.h" +#include "blackbox_io.h" + #include "build/debug.h" #include "build/version.h" @@ -31,8 +34,10 @@ #include "common/encoding.h" #include "common/utils.h" -#include "blackbox.h" -#include "blackbox_io.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/compass.h" @@ -43,18 +48,22 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" #include "flight/pid.h" #include "io/beeper.h" +#include "io/serial.h" -#include "sensors/sensors.h" +#include "rx/rx.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" #include "sensors/compass.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" #include "sensors/sonar.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define SLOW_FRAME_INTERVAL 4096 @@ -761,16 +770,16 @@ void validateBlackboxConfig() if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0 || blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) { - blackboxConfig()->rate_num = 1; - blackboxConfig()->rate_denom = 1; + blackboxConfigMutable()->rate_num = 1; + blackboxConfigMutable()->rate_denom = 1; } else { /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat * itself more frequently) */ div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom); - blackboxConfig()->rate_num /= div; - blackboxConfig()->rate_denom /= div; + blackboxConfigMutable()->rate_num /= div; + blackboxConfigMutable()->rate_denom /= div; } // If we've chosen an unsupported device, change the device to serial @@ -786,7 +795,7 @@ void validateBlackboxConfig() break; default: - blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL; + blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL; } } @@ -820,7 +829,7 @@ void startBlackbox(void) */ blackboxBuildConditionCache(); - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX); + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); blackboxIteration = 0; blackboxPFrameIndex = 0; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 388ca2f75d..d085f531c4 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -21,6 +21,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + typedef struct blackboxConfig_s { uint8_t rate_num; uint8_t rate_denom; @@ -28,6 +30,8 @@ typedef struct blackboxConfig_s { uint8_t on_motor_test; } blackboxConfig_t; +PG_DECLARE(blackboxConfig_t, blackboxConfig); + void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void initBlackbox(void); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 307ada96c1..356a98e5ee 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -8,6 +8,7 @@ #ifdef BLACKBOX +#include "blackbox.h" #include "blackbox_io.h" #include "build/version.h" @@ -16,6 +17,10 @@ #include "common/encoding.h" #include "common/printf.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" #include "fc/rc_controls.h" @@ -23,12 +28,10 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "io/serial.h" #include "msp/msp_serial.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #define BLACKBOX_SERIAL_PORT_MODE MODE_TX // How many bytes can we transmit per loop iteration when writing headers? diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 33b01b35d1..ae791cd1fc 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -44,19 +44,24 @@ #include "drivers/system.h" +// For rcData, stopAllMotors, stopPwmAllMotors +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + // For 'ARM' related #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -// For rcData, stopAllMotors, stopPwmAllMotors -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" +#include "flight/mixer.h" -// For VISIBLE* (Actually, included by config_master.h) +// For VISIBLE* #include "io/osd.h" +#include "rx/rx.h" + // DisplayPort management #ifndef CMS_MAX_DEVICE diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index d1ea564a88..b109e64562 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -27,10 +27,10 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" #include "cms/cms.h" @@ -40,8 +40,9 @@ #include "common/utils.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 5899c69f87..7dc533ddd8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -44,8 +44,9 @@ #include "flight/pid.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" // // PID diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index 45f9863f8c..e92f42c99a 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -22,15 +22,16 @@ #include "platform.h" -#include "build/version.h" - #ifdef CMS +#include "build/version.h" + #include "drivers/system.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "cms/cms.h" #include "cms/cms_types.h" diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 38d1e07160..05e1e54905 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -28,14 +28,15 @@ #include "drivers/system.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - #include "cms/cms.h" #include "cms/cms_types.h" #include "cms/cms_menu_ledstrip.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + // // Misc // diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 2d8b88f0ac..28be0d4743 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -21,6 +21,8 @@ #include "platform.h" +#if defined(OSD) && defined(CMS) + #include "build/version.h" #include "cms/cms.h" @@ -28,10 +30,9 @@ #include "cms/cms_menu_osd.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" - -#if defined(OSD) && defined(CMS) +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5}; OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50}; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index 066fe8eeca..a07a776b1a 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -21,6 +21,10 @@ #include "platform.h" +#ifdef CMS + +#if defined(VTX) || defined(USE_RTC6705) + #include "build/version.h" #include "cms/cms.h" @@ -30,12 +34,9 @@ #include "common/utils.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" - -#ifdef CMS - -#if defined(VTX) || defined(USE_RTC6705) +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" static bool featureRead = false; static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b6ef8cd90f..584c18876e 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -42,10 +42,10 @@ #include "flight/servos.h" #include "flight/imu.h" #include "flight/navigation.h" +#include "flight/pid.h" #include "io/serial.h" #include "io/gimbal.h" -#include "io/motors.h" #include "io/servos.h" #include "io/gps.h" #include "io/osd.h" @@ -64,6 +64,7 @@ #include "sensors/battery.h" #include "sensors/compass.h" +#ifndef USE_PARAMETER_GROUPS #define motorConfig(x) (&masterConfig.motorConfig) #define flight3DConfig(x) (&masterConfig.flight3DConfig) #define servoConfig(x) (&masterConfig.servoConfig) @@ -105,11 +106,71 @@ #define modeActivationProfile(x) (&masterConfig.modeActivationProfile) #define servoProfile(x) (&masterConfig.servoProfile) #define customMotorMixer(i) (&masterConfig.customMotorMixer[i]) -#define customServoMixer(i) (&masterConfig.customServoMixer[i]) +#define customServoMixers(i) (&masterConfig.customServoMixer[i]) #define displayPortProfileMsp(x) (&masterConfig.displayPortProfileMsp) #define displayPortProfileMax7456(x) (&masterConfig.displayPortProfileMax7456) #define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled) + +#define motorConfigMutable(x) (&masterConfig.motorConfig) +#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig) +#define servoConfigMutable(x) (&masterConfig.servoConfig) +#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig) +#define gimbalConfigMutable(x) (&masterConfig.gimbalConfig) +#define boardAlignmentMutable(x) (&masterConfig.boardAlignment) +#define imuConfigMutable(x) (&masterConfig.imuConfig) +#define gyroConfigMutable(x) (&masterConfig.gyroConfig) +#define compassConfigMutable(x) (&masterConfig.compassConfig) +#define accelerometerConfigMutable(x) (&masterConfig.accelerometerConfig) +#define barometerConfigMutable(x) (&masterConfig.barometerConfig) +#define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig) +#define batteryConfigMutable(x) (&masterConfig.batteryConfig) +#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig) +#define gpsProfileMutable(x) (&masterConfig.gpsProfile) +#define gpsConfigMutable(x) (&masterConfig.gpsConfig) +#define rxConfigMutable(x) (&masterConfig.rxConfig) +#define armingConfigMutable(x) (&masterConfig.armingConfig) +#define mixerConfigMutable(x) (&masterConfig.mixerConfig) +#define airplaneConfigMutable(x) (&masterConfig.airplaneConfig) +#define failsafeConfigMutable(x) (&masterConfig.failsafeConfig) +#define serialConfigMutable(x) (&masterConfig.serialConfig) +#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig) +#define ibusTelemetryConfigMutable(x) (&masterConfig.telemetryConfig) +#define ppmConfigMutable(x) (&masterConfig.ppmConfig) +#define pwmConfigMutable(x) (&masterConfig.pwmConfig) +#define adcConfigMutable(x) (&masterConfig.adcConfig) +#define beeperConfigMutable(x) (&masterConfig.beeperConfig) +#define sonarConfigMutable(x) (&masterConfig.sonarConfig) +#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig) +#define statusLedConfigMutable(x) (&masterConfig.statusLedConfig) +#define osdProfileMutable(x) (&masterConfig.osdProfile) +#define vcdProfileMutable(x) (&masterConfig.vcdProfile) +#define sdcardConfigMutable(x) (&masterConfig.sdcardConfig) +#define blackboxConfigMutable(x) (&masterConfig.blackboxConfig) +#define flashConfigMutable(x) (&masterConfig.flashConfig) +#define pidConfigMutable(x) (&masterConfig.pidConfig) +#define adjustmentProfileMutable(x) (&masterConfig.adjustmentProfile) +#define modeActivationProfileMutable(x) (&masterConfig.modeActivationProfile) +#define servoProfileMutable(x) (&masterConfig.servoProfile) +#define customMotorMixerMutable(i) (&masterConfig.customMotorMixer[i]) +#define customServoMixersMutable(i) (&masterConfig.customServoMixer[i]) +#define displayPortProfileMspMutable(x) (&masterConfig.displayPortProfileMsp) +#define displayPortProfileMax7456Mutable(x) (&masterConfig.displayPortProfileMax7456) +#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled) + +#define servoParams(x) (&servoProfile()->servoConf[i]) +#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x]) +#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x]) +#define osdConfig(x) (&masterConfig.osdProfile) +#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) + +#define servoParamsMutable(x) (&servoProfile()->servoConf[i]) +#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[i]) +#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x]) +#define osdConfigMutable(x) (&masterConfig.osdProfile) +#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x]) +#endif + // System-wide typedef struct master_s { uint8_t version; diff --git a/src/main/config/feature.c b/src/main/config/feature.c index 44b3c12d8b..508e5abeb1 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -21,8 +21,9 @@ #include "platform.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" static uint32_t activeFeaturesLatch = 0; diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 11f6ff355f..2eb75f8a42 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -17,6 +17,14 @@ #pragma once +#include "config/parameter_group.h" + +typedef struct featureConfig_s { + uint32_t enabledFeatures; +} featureConfig_t; + +PG_DECLARE(featureConfig_t, featureConfig); + void latchActiveFeatures(void); bool featureConfigured(uint32_t mask); bool feature(uint32_t mask); @@ -27,4 +35,4 @@ uint32_t featureMask(void); void intFeatureClearAll(uint32_t *features); void intFeatureSet(uint32_t mask, uint32_t *features); -void intFeatureClear(uint32_t mask, uint32_t *features); \ No newline at end of file +void intFeatureClear(uint32_t mask, uint32_t *features); diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c index 2711639cc1..58dbe51e1a 100644 --- a/src/main/config/parameter_group.c +++ b/src/main/config/parameter_group.c @@ -19,6 +19,8 @@ #include #include +#include "platform.h" + #include "parameter_group.h" #include "common/maths.h" @@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex) } } } - diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 2533b50158..8ee75f9968 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -100,6 +100,7 @@ extern const uint8_t __pg_resetdata_end[]; } while(0) \ /**/ +#ifdef USE_PARAMETER_GROUPS // Declare system config #define PG_DECLARE(_type, _name) \ extern _type _name ## _System; \ @@ -125,6 +126,20 @@ extern const uint8_t __pg_resetdata_end[]; struct _dummy \ /**/ +#else + +#define PG_DECLARE(_type, _name) \ + extern _type _name ## _System + +#define PG_DECLARE_ARRAY(_type, _size, _name) \ + extern _type _name ## _SystemArray[_size] + +// Declare profile config +#define PG_DECLARE_PROFILE(_type, _name) \ + extern _type *_name ## _ProfileCurrent + +#endif // USE_PARAMETER_GROUPS + // Register system config #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ _type _name ## _System; \ diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index fbef2e84d5..44ad13b037 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -15,6 +15,10 @@ * along with Cleanflight. If not, see . */ +#ifndef USE_PARAMETER_GROUPS +#include "config/config_master.h" +#endif + // FC configuration #define PG_FAILSAFE_CONFIG 1 // struct OK #define PG_BOARD_ALIGNMENT 2 // struct OK diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d6507da396..b6f9265b80 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -32,6 +32,8 @@ uint8_t cliMode = 0; #ifdef USE_CLI +#include "blackbox/blackbox.h" + #include "build/build_config.h" #include "build/debug.h" #include "build/version.h" @@ -45,9 +47,10 @@ uint8_t cliMode = 0; #include "common/typeconversion.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 "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -81,6 +84,7 @@ uint8_t cliMode = 0; #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" @@ -823,6 +827,71 @@ static const clivalue_t valueTable[] = { }; #endif +#ifdef USE_PARAMETER_GROUPS +static featureConfig_t featureConfigCopy; +static gyroConfig_t gyroConfigCopy; +static accelerometerConfig_t accelerometerConfigCopy; +#ifdef MAG +static compassConfig_t compassConfigCopy; +#endif +#ifdef BARO +static barometerConfig_t barometerConfigCopy; +#endif +#ifdef PITOT +static pitotmeterConfig_t pitotmeterConfigCopy; +#endif +static featureConfig_t featureConfigCopy; +static rxConfig_t rxConfigCopy; +#ifdef BLACKBOX +static blackboxConfig_t blackboxConfigCopy; +#endif +static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT]; +static motorConfig_t motorConfigCopy; +static failsafeConfig_t failsafeConfigCopy; +static boardAlignment_t boardAlignmentCopy; +#ifdef USE_SERVOS +static servoConfig_t servoConfigCopy; +static gimbalConfig_t gimbalConfigCopy; +static servoMixer_t customServoMixersCopy[MAX_SERVO_RULES]; +static servoParam_t servoParamsCopy[MAX_SUPPORTED_SERVOS]; +#endif +static batteryConfig_t batteryConfigCopy; +static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS]; +static mixerConfig_t mixerConfigCopy; +static flight3DConfig_t flight3DConfigCopy; +static serialConfig_t serialConfigCopy; +static imuConfig_t imuConfigCopy; +static armingConfig_t armingConfigCopy; +static rcControlsConfig_t rcControlsConfigCopy; +#ifdef GPS +static gpsConfig_t gpsConfigCopy; +#endif +#ifdef NAV +static positionEstimationConfig_t positionEstimationConfigCopy; +static navConfig_t navConfigCopy; +#endif +#ifdef TELEMETRY +static telemetryConfig_t telemetryConfigCopy; +#endif +static modeActivationCondition_t modeActivationConditionsCopy[MAX_MODE_ACTIVATION_CONDITION_COUNT]; +static adjustmentRange_t adjustmentRangesCopy[MAX_ADJUSTMENT_RANGE_COUNT]; +#ifdef LED_STRIP +static ledStripConfig_t ledStripConfigCopy; +#endif +#ifdef OSD +static osdConfig_t osdConfigCopy; +#endif +static systemConfig_t systemConfigCopy; +#ifdef BEEPER +static beeperConfig_t beeperConfigCopy; +#endif +static controlRateConfig_t controlRateProfilesCopy[MAX_CONTROL_RATE_PROFILE_COUNT]; +static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT]; +static modeActivationOperatorConfig_t modeActivationOperatorConfigCopy; +static beeperConfig_t beeperConfigCopy; +#endif // USE_PARAMETER_GROUPS + static void cliPrint(const char *str) { while (*str) { @@ -994,6 +1063,180 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn static cliCurrentAndDefaultConfig_t ret; switch (pgn) { + case PG_GYRO_CONFIG: + ret.currentConfig = &gyroConfigCopy; + ret.defaultConfig = gyroConfig(); + break; + case PG_ACCELEROMETER_CONFIG: + ret.currentConfig = &accelerometerConfigCopy; + ret.defaultConfig = accelerometerConfig(); + break; +#ifdef MAG + case PG_COMPASS_CONFIG: + ret.currentConfig = &compassConfigCopy; + ret.defaultConfig = compassConfig(); + break; +#endif +#ifdef BARO + case PG_BAROMETER_CONFIG: + ret.currentConfig = &barometerConfigCopy; + ret.defaultConfig = barometerConfig(); + break; +#endif +#ifdef PITOT + case PG_PITOTMETER_CONFIG: + ret.currentConfig = &pitotmeterConfigCopy; + ret.defaultConfig = pitotmeterConfig(); + break; +#endif + case PG_FEATURE_CONFIG: + ret.currentConfig = &featureConfigCopy; + ret.defaultConfig = featureConfig(); + break; + case PG_RX_CONFIG: + ret.currentConfig = &rxConfigCopy; + ret.defaultConfig = rxConfig(); + break; +#ifdef BLACKBOX + case PG_BLACKBOX_CONFIG: + ret.currentConfig = &blackboxConfigCopy; + ret.defaultConfig = blackboxConfig(); + break; +#endif + case PG_MOTOR_CONFIG: + ret.currentConfig = &motorConfigCopy; + ret.defaultConfig = motorConfig(); + break; + case PG_FAILSAFE_CONFIG: + ret.currentConfig = &failsafeConfigCopy; + ret.defaultConfig = failsafeConfig(); + break; + case PG_BOARD_ALIGNMENT: + ret.currentConfig = &boardAlignmentCopy; + ret.defaultConfig = boardAlignment(); + break; + case PG_MIXER_CONFIG: + ret.currentConfig = &mixerConfigCopy; + ret.defaultConfig = mixerConfig(); + break; + case PG_MOTOR_3D_CONFIG: + ret.currentConfig = &flight3DConfigCopy; + ret.defaultConfig = flight3DConfig(); + break; +#ifdef USE_SERVOS + case PG_SERVO_CONFIG: + ret.currentConfig = &servoConfigCopy; + ret.defaultConfig = servoConfig(); + break; + case PG_GIMBAL_CONFIG: + ret.currentConfig = &gimbalConfigCopy; + ret.defaultConfig = gimbalConfig(); + break; +#endif + case PG_BATTERY_CONFIG: + ret.currentConfig = &batteryConfigCopy; + ret.defaultConfig = batteryConfig(); + break; + case PG_SERIAL_CONFIG: + ret.currentConfig = &serialConfigCopy; + ret.defaultConfig = serialConfig(); + break; + case PG_IMU_CONFIG: + ret.currentConfig = &imuConfigCopy; + ret.defaultConfig = imuConfig(); + break; + case PG_RC_CONTROLS_CONFIG: + ret.currentConfig = &rcControlsConfigCopy; + ret.defaultConfig = rcControlsConfig(); + break; + case PG_ARMING_CONFIG: + ret.currentConfig = &armingConfigCopy; + ret.defaultConfig = armingConfig(); + break; +#ifdef GPS + case PG_GPS_CONFIG: + ret.currentConfig = &gpsConfigCopy; + ret.defaultConfig = gpsConfig(); + break; +#endif +#ifdef NAV + case PG_POSITION_ESTIMATION_CONFIG: + ret.currentConfig = &positionEstimationConfigCopy; + ret.defaultConfig = positionEstimationConfig(); + break; + case PG_NAV_CONFIG: + ret.currentConfig = &navConfigCopy; + ret.defaultConfig = navConfig(); + break; +#endif +#ifdef TELEMETRY + case PG_TELEMETRY_CONFIG: + ret.currentConfig = &telemetryConfigCopy; + ret.defaultConfig = telemetryConfig(); + break; +#endif +#ifdef LED_STRIP + case PG_LED_STRIP_CONFIG: + ret.currentConfig = &ledStripConfigCopy; + ret.defaultConfig = ledStripConfig(); + break; +#endif +#ifdef OSD + case PG_OSD_CONFIG: + ret.currentConfig = &osdConfigCopy; + ret.defaultConfig = osdConfig(); + break; +#endif + case PG_SYSTEM_CONFIG: + ret.currentConfig = &systemConfigCopy; + ret.defaultConfig = systemConfig(); + break; + case PG_MODE_ACTIVATION_OPERATOR_CONFIG: + ret.currentConfig = &modeActivationOperatorConfigCopy; + ret.defaultConfig = modeActivationOperatorConfig(); + break; + case PG_CONTROL_RATE_PROFILES: + ret.currentConfig = &controlRateProfilesCopy[0]; + ret.defaultConfig = controlRateProfiles(0); + break; + case PG_PID_PROFILE: + ret.currentConfig = &pidProfileCopy[getConfigProfile()]; + ret.defaultConfig = pidProfile(); + break; + case PG_RX_FAILSAFE_CHANNEL_CONFIG: + ret.currentConfig = &rxFailsafeChannelConfigsCopy[0]; + ret.defaultConfig = rxFailsafeChannelConfigs(0); + break; + case PG_RX_CHANNEL_RANGE_CONFIG: + ret.currentConfig = &rxChannelRangeConfigsCopy[0]; + ret.defaultConfig = rxChannelRangeConfigs(0); + break; +#ifdef USE_SERVOS + case PG_SERVO_MIXER: + ret.currentConfig = &customServoMixersCopy[0]; + ret.defaultConfig = customServoMixers(0); + break; + case PG_SERVO_PARAMS: + ret.currentConfig = &servoParamsCopy[0]; + ret.defaultConfig = servoParams(0); + break; +#endif + case PG_MOTOR_MIXER: + ret.currentConfig = &customMotorMixerCopy[0]; + ret.defaultConfig = customMotorMixer(0); + break; + case PG_MODE_ACTIVATION_PROFILE: + ret.currentConfig = &modeActivationConditionsCopy[0]; + ret.defaultConfig = modeActivationConditions(0); + break; + case PG_ADJUSTMENT_RANGE_CONFIG: + ret.currentConfig = &adjustmentRangesCopy[0]; + ret.defaultConfig = adjustmentRanges(0); + break; + case PG_BEEPER_CONFIG: + ret.currentConfig = &beeperConfigCopy; + ret.defaultConfig = beeperConfig(); + break; default: ret.currentConfig = NULL; ret.defaultConfig = NULL; @@ -1810,7 +2053,7 @@ static void cliMotorMix(char *cmdline) } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - customMotorMixer(i)->throttle = 0.0f; + customMotorMixerMutable(i)->throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = nextArg(cmdline); if (ptr) { @@ -1821,7 +2064,7 @@ static void cliMotorMix(char *cmdline) break; } if (strncasecmp(ptr, mixerNames[i], len) == 0) { - mixerLoadMix(i, masterConfig.customMotorMixer); + mixerLoadMix(i, customMotorMixerMutable(0)); cliPrintf("Loaded %s\r\n", mixerNames[i]); cliMotorMix(""); break; @@ -1834,22 +2077,22 @@ static void cliMotorMix(char *cmdline) if (i < MAX_SUPPORTED_MOTORS) { ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->throttle = fastA2F(ptr); + customMotorMixerMutable(i)->throttle = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->roll = fastA2F(ptr); + customMotorMixerMutable(i)->roll = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->pitch = fastA2F(ptr); + customMotorMixerMutable(i)->pitch = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixer(i)->yaw = fastA2F(ptr); + customMotorMixerMutable(i)->yaw = fastA2F(ptr); check++; } if (check != 4) { @@ -2200,7 +2443,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig) { const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { - servoMixer_t customServoMixer = *customServoMixer(i); + const servoMixer_t customServoMixer = *customServoMixers(i); if (customServoMixer.rate == 0) { break; } @@ -2361,13 +2604,13 @@ static void cliServoMix(char *cmdline) args[MIN] >= 0 && args[MIN] <= 100 && args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - customServoMixer(i)->targetChannel = args[TARGET]; - customServoMixer(i)->inputSource = args[INPUT]; - customServoMixer(i)->rate = args[RATE]; - customServoMixer(i)->speed = args[SPEED]; - customServoMixer(i)->min = args[MIN]; - customServoMixer(i)->max = args[MAX]; - customServoMixer(i)->box = args[BOX]; + customServoMixersMutable(i)->targetChannel = args[TARGET]; + customServoMixersMutable(i)->inputSource = args[INPUT]; + customServoMixersMutable(i)->rate = args[RATE]; + customServoMixersMutable(i)->speed = args[SPEED]; + customServoMixersMutable(i)->min = args[MIN]; + customServoMixersMutable(i)->max = args[MAX]; + customServoMixersMutable(i)->box = args[BOX]; cliServoMix(""); } else { cliShowParseError(); @@ -3023,7 +3266,7 @@ static void cliMixer(char *cmdline) return; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - mixerConfig()->mixerMode = i + 1; + mixerConfigMutable()->mixerMode = i + 1; break; } } @@ -3671,12 +3914,45 @@ static void cliResource(char *cmdline) #ifdef USE_PARAMETER_GROUPS static void backupConfigs(void) { - // make copies of configs to do differencing - + // make copies of configs to do differencing + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("BACKUP %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(&pidProfileCopy[0], reg->address, sizeof(pidProfile_t) * MAX_PROFILE_COUNT); } static void restoreConfigs(void) { + PG_FOREACH(reg) { + // currentConfig is the copy + const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg)); + if (cliCurrentAndDefaultConfig->currentConfig) { + if (pgIsProfile(reg)) { + //memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size * MAX_PROFILE_COUNT); + } else { + memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size); + } +#ifdef SERIAL_CLI_DEBUG + } else { + cliPrintf("RESTORE %d SET UP INCORRECTLY\r\n", pgN(reg)); +#endif + } + } + const pgRegistry_t* reg = pgFind(PG_PID_PROFILE); + memcpy(reg->address, &pidProfileCopy[0], sizeof(pidProfile_t) * MAX_PROFILE_COUNT); } #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8ee0519c81..06066752e0 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -951,7 +951,7 @@ void activateConfig(void) void validateAndFixConfig(void) { if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){ - motorConfig()->mincommand = 1000; + motorConfigMutable()->mincommand = 1000; } if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { @@ -1064,18 +1064,18 @@ void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { - gyroConfig()->gyro_soft_notch_hz_1 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; } if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) { - gyroConfig()->gyro_soft_notch_hz_2 = 0; + gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; } float samplingTime = 0.000125f; if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { - pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - gyroConfig()->gyro_sync_denom = 1; - gyroConfig()->gyro_use_32khz = false; + pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + gyroConfigMutable()->gyro_sync_denom = 1; + gyroConfigMutable()->gyro_use_32khz = false; samplingTime = 0.001f; } @@ -1094,7 +1094,7 @@ void validateAndFixGyroConfig(void) } #if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfig()->gyro_isr_update = false; + gyroConfigMutable()->gyro_isr_update = false; #endif // check for looptime restrictions based on motor protocol. Motor times have safety margin @@ -1123,14 +1123,14 @@ void validateAndFixGyroConfig(void) } if(pidLooptime < motorUpdateRestriction) - pidConfig()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom); + pidConfigMutable()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom); // Prevent overriding the max rate of motors if(motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED)) { uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); if(motorConfig()->motorPwmRate > maxEscRate) - motorConfig()->motorPwmRate = maxEscRate; + motorConfigMutable()->motorPwmRate = maxEscRate; } } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 7a85b85384..3b69a1f189 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -61,6 +61,12 @@ typedef enum { FEATURE_ESC_SENSOR = 1 << 27, } features_e; +typedef struct systemConfig_s { + uint8_t debug_mode; +} systemConfig_t; + +//!!TODOPG_DECLARE(systemConfig_t, systemConfig); + void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); void beeperOffClear(uint32_t mask); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 94897ca306..eea39c5f2d 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -29,15 +29,21 @@ #include "common/utils.h" #include "common/filter.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/light_led.h" #include "drivers/system.h" #include "drivers/gyro_sync.h" -#include "sensors/sensors.h" -#include "sensors/boardalignment.h" #include "sensors/acceleration.h" -#include "sensors/gyro.h" +#include "sensors/barometer.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" #include "fc/config.h" #include "fc/rc_controls.h" @@ -60,15 +66,14 @@ #include "scheduler/scheduler.h" +#include "telemetry/telemetry.h" + #include "flight/mixer.h" #include "flight/servos.h" #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" // June 2013 V2.2-dev @@ -96,8 +101,8 @@ static bool armingCalibrationWasInitialised; void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { - accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; - accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; + accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; + accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; saveConfigAndNotify(); } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7d0dfbc86e..a5224e8508 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -28,6 +28,12 @@ #include "common/maths.h" #include "common/printf.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "cms/cms.h" #include "cms/cms_types.h" @@ -115,10 +121,6 @@ #include "flight/failsafe.h" #include "flight/navigation.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" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 45587418d7..dbf8815a72 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -36,9 +36,11 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "drivers/system.h" #include "drivers/accgyro.h" #include "drivers/bus_i2c.h" #include "drivers/compass.h" @@ -673,25 +675,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_SERVO_CONFIGURATIONS: for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - sbufWriteU16(dst, servoProfile()->servoConf[i].min); - sbufWriteU16(dst, servoProfile()->servoConf[i].max); - sbufWriteU16(dst, servoProfile()->servoConf[i].middle); - sbufWriteU8(dst, servoProfile()->servoConf[i].rate); - sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin); - sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax); - sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel); - sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources); + sbufWriteU16(dst, servoParams(i)->min); + sbufWriteU16(dst, servoParams(i)->max); + sbufWriteU16(dst, servoParams(i)->middle); + sbufWriteU8(dst, servoParams(i)->rate); + sbufWriteU8(dst, servoParams(i)->angleAtMin); + sbufWriteU8(dst, servoParams(i)->angleAtMax); + sbufWriteU8(dst, servoParams(i)->forwardFromChannel); + sbufWriteU32(dst, servoParams(i)->reversedSources); } break; case MSP_SERVO_MIX_RULES: for (int i = 0; i < MAX_SERVO_RULES; i++) { - sbufWriteU8(dst, customServoMixer(i)->targetChannel); - sbufWriteU8(dst, customServoMixer(i)->inputSource); - sbufWriteU8(dst, customServoMixer(i)->rate); - sbufWriteU8(dst, customServoMixer(i)->speed); - sbufWriteU8(dst, customServoMixer(i)->min); - sbufWriteU8(dst, customServoMixer(i)->max); - sbufWriteU8(dst, customServoMixer(i)->box); + sbufWriteU8(dst, customServoMixers(i)->targetChannel); + sbufWriteU8(dst, customServoMixers(i)->inputSource); + sbufWriteU8(dst, customServoMixers(i)->rate); + sbufWriteU8(dst, customServoMixers(i)->speed); + sbufWriteU8(dst, customServoMixers(i)->min); + sbufWriteU8(dst, customServoMixers(i)->max); + sbufWriteU8(dst, customServoMixers(i)->box); } break; #endif @@ -798,7 +800,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_ADJUSTMENT_RANGES: for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + const adjustmentRange_t *adjRange = adjustmentRanges(i); sbufWriteU8(dst, adjRange->adjustmentIndex); sbufWriteU8(dst, adjRange->auxChannelIndex); sbufWriteU8(dst, adjRange->range.startStep); @@ -965,8 +967,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_RXFAIL_CONFIG: for (int i = 0; i < rxRuntimeConfig.channelCount; i++) { - sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode); - sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step)); + sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode); + sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step)); } break; @@ -1012,7 +1014,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #ifdef LED_STRIP case MSP_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + const hsvColor_t *color = &ledStripConfig()->colors[i]; sbufWriteU16(dst, color->h); sbufWriteU8(dst, color->s); sbufWriteU8(dst, color->v); @@ -1021,7 +1023,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_LED_STRIP_CONFIG: for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; sbufWriteU32(dst, *ledConfig); } break; @@ -1089,13 +1091,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #else sbufWriteU8(dst, 0); #endif - sbufWriteU8(dst, osdProfile()->units); - sbufWriteU8(dst, osdProfile()->rssi_alarm); - sbufWriteU16(dst, osdProfile()->cap_alarm); - sbufWriteU16(dst, osdProfile()->time_alarm); - sbufWriteU16(dst, osdProfile()->alt_alarm); + sbufWriteU8(dst, osdConfig()->units); + sbufWriteU8(dst, osdConfig()->rssi_alarm); + sbufWriteU16(dst, osdConfig()->cap_alarm); + sbufWriteU16(dst, osdConfig()->time_alarm); + sbufWriteU16(dst, osdConfig()->alt_alarm); for (int i = 0; i < OSD_ITEM_COUNT; i++) { - sbufWriteU16(dst, osdProfile()->item_pos[i]); + sbufWriteU16(dst, osdConfig()->item_pos[i]); } #else sbufWriteU8(dst, 0); // OSD not supported @@ -1315,12 +1317,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif break; case MSP_SET_ACC_TRIM: - accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src); - accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src); + accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src); + accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(src); break; case MSP_SET_ARMING_CONFIG: - armingConfig()->auto_disarm_delay = sbufReadU8(src); - armingConfig()->disarm_kill_switch = sbufReadU8(src); + armingConfigMutable()->auto_disarm_delay = sbufReadU8(src); + armingConfigMutable()->disarm_kill_switch = sbufReadU8(src); break; case MSP_SET_LOOP_TIME: @@ -1342,7 +1344,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_MODE_RANGE: i = sbufReadU8(src); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i]; + modeActivationCondition_t *mac = modeActivationConditionsMutable(i); i = sbufReadU8(src); const box_t *box = findBoxByPermenantId(i); if (box) { @@ -1351,7 +1353,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), motorConfig(), ¤tProfile->pidProfile); } else { return MSP_RESULT_ERROR; } @@ -1363,7 +1365,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_ADJUSTMENT_RANGE: i = sbufReadU8(src); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i]; + adjustmentRange_t *adjRange = adjustmentRangesMutable(i); i = sbufReadU8(src); if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { adjRange->adjustmentIndex = i; @@ -1405,32 +1407,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_MISC: - rxConfig()->midrc = sbufReadU16(src); - motorConfig()->minthrottle = sbufReadU16(src); - motorConfig()->maxthrottle = sbufReadU16(src); - motorConfig()->mincommand = sbufReadU16(src); + rxConfigMutable()->midrc = sbufReadU16(src); + motorConfigMutable()->minthrottle = sbufReadU16(src); + motorConfigMutable()->maxthrottle = sbufReadU16(src); + motorConfigMutable()->mincommand = sbufReadU16(src); - failsafeConfig()->failsafe_throttle = sbufReadU16(src); + failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); #ifdef GPS - gpsConfig()->provider = sbufReadU8(src); // gps_type + gpsConfigMutable()->provider = sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate - gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas + gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas #else sbufReadU8(src); // gps_type sbufReadU8(src); // gps_baudrate sbufReadU8(src); // gps_ubx_sbas #endif - batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src); - rxConfig()->rssi_channel = sbufReadU8(src); + batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src); + rxConfigMutable()->rssi_channel = sbufReadU8(src); sbufReadU8(src); - compassConfig()->mag_declination = sbufReadU16(src) * 10; + compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; - batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert break; case MSP_SET_MOTOR: @@ -1448,14 +1450,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= MAX_SUPPORTED_SERVOS) { return MSP_RESULT_ERROR; } else { - servoProfile()->servoConf[i].min = sbufReadU16(src); - servoProfile()->servoConf[i].max = sbufReadU16(src); - servoProfile()->servoConf[i].middle = sbufReadU16(src); - servoProfile()->servoConf[i].rate = sbufReadU8(src); - servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src); - servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src); - servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src); - servoProfile()->servoConf[i].reversedSources = sbufReadU32(src); + servoParamsMutable(i)->min = sbufReadU16(src); + servoParamsMutable(i)->max = sbufReadU16(src); + servoParamsMutable(i)->middle = sbufReadU16(src); + servoParamsMutable(i)->rate = sbufReadU8(src); + servoParamsMutable(i)->angleAtMin = sbufReadU8(src); + servoParamsMutable(i)->angleAtMax = sbufReadU8(src); + servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src); + servoParamsMutable(i)->reversedSources = sbufReadU32(src); } #endif break; @@ -1466,59 +1468,59 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= MAX_SERVO_RULES) { return MSP_RESULT_ERROR; } else { - customServoMixer(i)->targetChannel = sbufReadU8(src); - customServoMixer(i)->inputSource = sbufReadU8(src); - customServoMixer(i)->rate = sbufReadU8(src); - customServoMixer(i)->speed = sbufReadU8(src); - customServoMixer(i)->min = sbufReadU8(src); - customServoMixer(i)->max = sbufReadU8(src); - customServoMixer(i)->box = sbufReadU8(src); + customServoMixersMutable(i)->targetChannel = sbufReadU8(src); + customServoMixersMutable(i)->inputSource = sbufReadU8(src); + customServoMixersMutable(i)->rate = sbufReadU8(src); + customServoMixersMutable(i)->speed = sbufReadU8(src); + customServoMixersMutable(i)->min = sbufReadU8(src); + customServoMixersMutable(i)->max = sbufReadU8(src); + customServoMixersMutable(i)->box = sbufReadU8(src); loadCustomServoMixer(); } #endif break; case MSP_SET_3D: - flight3DConfig()->deadband3d_low = sbufReadU16(src); - flight3DConfig()->deadband3d_high = sbufReadU16(src); - flight3DConfig()->neutral3d = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_low = sbufReadU16(src); + flight3DConfigMutable()->deadband3d_high = sbufReadU16(src); + flight3DConfigMutable()->neutral3d = sbufReadU16(src); break; case MSP_SET_RC_DEADBAND: - rcControlsConfig()->deadband = sbufReadU8(src); - rcControlsConfig()->yaw_deadband = sbufReadU8(src); - rcControlsConfig()->alt_hold_deadband = sbufReadU8(src); - flight3DConfig()->deadband3d_throttle = sbufReadU16(src); + rcControlsConfigMutable()->deadband = sbufReadU8(src); + rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); + rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); + flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RESET_CURR_PID: resetProfile(currentProfile); break; case MSP_SET_SENSOR_ALIGNMENT: - gyroConfig()->gyro_align = sbufReadU8(src); - accelerometerConfig()->acc_align = sbufReadU8(src); - compassConfig()->mag_align = sbufReadU8(src); + gyroConfigMutable()->gyro_align = sbufReadU8(src); + accelerometerConfigMutable()->acc_align = sbufReadU8(src); + compassConfigMutable()->mag_align = sbufReadU8(src); break; case MSP_SET_ADVANCED_CONFIG: - gyroConfig()->gyro_sync_denom = sbufReadU8(src); - pidConfig()->pid_process_denom = sbufReadU8(src); - motorConfig()->useUnsyncedPwm = sbufReadU8(src); + gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src); + pidConfigMutable()->pid_process_denom = sbufReadU8(src); + motorConfigMutable()->useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT - motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); + motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); #else - motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); + motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif motorConfig()->motorPwmRate = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } if (sbufBytesRemaining(src)) { - gyroConfig()->gyro_use_32khz = sbufReadU8(src); + gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src); } //!!TODO gyro_isr_update to be added pending decision /*if (sbufBytesRemaining(src)) { - gyroConfig()->gyro_isr_update = sbufReadU8(src); + gyroConfigMutable()->gyro_isr_update = sbufReadU8(src); }*/ validateAndFixGyroConfig(); @@ -1528,18 +1530,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_FILTER_CONFIG: - gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src); + gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { - gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src); - gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src); } if (dataSize > 13) { - gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src); - gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src); + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src); } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); @@ -1569,9 +1571,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_SENSOR_CONFIG: - accelerometerConfig()->acc_hardware = sbufReadU8(src); - barometerConfig()->baro_hardware = sbufReadU8(src); - compassConfig()->mag_hardware = sbufReadU8(src); + accelerometerConfigMutable()->acc_hardware = sbufReadU8(src); + barometerConfigMutable()->baro_hardware = sbufReadU8(src); + compassConfigMutable()->mag_hardware = sbufReadU8(src); break; case MSP_RESET_CONF: @@ -1603,9 +1605,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_BLACKBOX_CONFIG: // Don't allow config to be updated while Blackbox is logging if (blackboxMayEditConfig()) { - blackboxConfig()->device = sbufReadU8(src); - blackboxConfig()->rate_num = sbufReadU8(src); - blackboxConfig()->rate_denom = sbufReadU8(src); + blackboxConfigMutable()->device = sbufReadU8(src); + blackboxConfigMutable()->rate_num = sbufReadU8(src); + blackboxConfigMutable()->rate_denom = sbufReadU8(src); } break; #endif @@ -1761,53 +1763,53 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_BOARD_ALIGNMENT: - boardAlignment()->rollDegrees = sbufReadU16(src); - boardAlignment()->pitchDegrees = sbufReadU16(src); - boardAlignment()->yawDegrees = sbufReadU16(src); + boardAlignmentMutable()->rollDegrees = sbufReadU16(src); + boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); + boardAlignmentMutable()->yawDegrees = sbufReadU16(src); break; case MSP_SET_VOLTAGE_METER_CONFIG: - batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended - batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended + batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert if (dataSize > 4) { - batteryConfig()->batteryMeterType = sbufReadU8(src); + batteryConfigMutable()->batteryMeterType = sbufReadU8(src); } break; case MSP_SET_CURRENT_METER_CONFIG: - batteryConfig()->currentMeterScale = sbufReadU16(src); - batteryConfig()->currentMeterOffset = sbufReadU16(src); - batteryConfig()->currentMeterType = sbufReadU8(src); - batteryConfig()->batteryCapacity = sbufReadU16(src); + batteryConfigMutable()->currentMeterScale = sbufReadU16(src); + batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); + batteryConfigMutable()->currentMeterType = sbufReadU8(src); + batteryConfigMutable()->batteryCapacity = sbufReadU16(src); break; #ifndef USE_QUAD_MIXER_ONLY case MSP_SET_MIXER: - mixerConfig()->mixerMode = sbufReadU8(src); + mixerConfigMutable()->mixerMode = sbufReadU8(src); break; #endif case MSP_SET_RX_CONFIG: - rxConfig()->serialrx_provider = sbufReadU8(src); - rxConfig()->maxcheck = sbufReadU16(src); - rxConfig()->midrc = sbufReadU16(src); - rxConfig()->mincheck = sbufReadU16(src); - rxConfig()->spektrum_sat_bind = sbufReadU8(src); + rxConfigMutable()->serialrx_provider = sbufReadU8(src); + rxConfigMutable()->maxcheck = sbufReadU16(src); + rxConfigMutable()->midrc = sbufReadU16(src); + rxConfigMutable()->mincheck = sbufReadU16(src); + rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); if (dataSize > 8) { - rxConfig()->rx_min_usec = sbufReadU16(src); - rxConfig()->rx_max_usec = sbufReadU16(src); + rxConfigMutable()->rx_min_usec = sbufReadU16(src); + rxConfigMutable()->rx_max_usec = sbufReadU16(src); } if (dataSize > 12) { - rxConfig()->rcInterpolation = sbufReadU8(src); - rxConfig()->rcInterpolationInterval = sbufReadU8(src); - rxConfig()->airModeActivateThreshold = sbufReadU16(src); + rxConfigMutable()->rcInterpolation = sbufReadU8(src); + rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src); + rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src); } if (dataSize > 16) { - rxConfig()->rx_spi_protocol = sbufReadU8(src); - rxConfig()->rx_spi_id = sbufReadU32(src); - rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src); + rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); + rxConfigMutable()->rx_spi_id = sbufReadU32(src); + rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); } if (dataSize > 22) { rxConfig()->fpvCamAngleDegrees = sbufReadU8(src); @@ -1815,31 +1817,31 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_FAILSAFE_CONFIG: - failsafeConfig()->failsafe_delay = sbufReadU8(src); - failsafeConfig()->failsafe_off_delay = sbufReadU8(src); - failsafeConfig()->failsafe_throttle = sbufReadU16(src); - failsafeConfig()->failsafe_kill_switch = sbufReadU8(src); - failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src); - failsafeConfig()->failsafe_procedure = sbufReadU8(src); + failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); + failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); + failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src); + failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); + failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src); break; case MSP_SET_RXFAIL_CONFIG: i = sbufReadU8(src); if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { - rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src); - rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); + rxConfigMutable()->failsafe_channel_configurations[i].mode = sbufReadU8(src); + rxConfigMutable()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); } else { return MSP_RESULT_ERROR; } break; case MSP_SET_RSSI_CONFIG: - rxConfig()->rssi_channel = sbufReadU8(src); + rxConfigMutable()->rssi_channel = sbufReadU8(src); break; case MSP_SET_RX_MAP: for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - rxConfig()->rcmap[i] = sbufReadU8(src); + rxConfigMutable()->rcmap[i] = sbufReadU8(src); } break; @@ -1847,20 +1849,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef USE_QUAD_MIXER_ONLY sbufReadU8(src); // mixerMode ignored #else - mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode + mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode #endif featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap - rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type + rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type - boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll - boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch - boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw + boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll + boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch + boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw - batteryConfig()->currentMeterScale = sbufReadU16(src); - batteryConfig()->currentMeterOffset = sbufReadU16(src); + batteryConfigMutable()->currentMeterScale = sbufReadU16(src); + batteryConfigMutable()->currentMeterOffset = sbufReadU16(src); break; case MSP_SET_CF_SERIAL_CONFIG: @@ -1894,7 +1896,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef LED_STRIP case MSP_SET_LED_COLORS: for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &ledStripConfig()->colors[i]; + hsvColor_t *color = &ledStripConfigMutable()->colors[i]; color->h = sbufReadU16(src); color->s = sbufReadU8(src); color->v = sbufReadU8(src); @@ -1907,7 +1909,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) { return MSP_RESULT_ERROR; } - ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i]; + ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i]; *ledConfig = sbufReadU32(src); reevaluateLedConfig(); } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index ec12ed8090..a729abe88a 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -27,6 +27,11 @@ #include "common/color.h" #include "common/utils.h" +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" @@ -43,8 +48,10 @@ #include "fc/cli.h" #include "fc/fc_dispatch.h" -#include "flight/pid.h" #include "flight/altitudehold.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/pid.h" #include "io/beeper.h" #include "io/dashboard.h" @@ -72,10 +79,6 @@ #include "telemetry/telemetry.h" -#include "config/feature.h" -#include "config/config_profile.h" -#include "config/config_master.h" - #ifdef USE_BST void taskBstMasterProcess(timeUs_t currentTimeUs); #endif @@ -94,7 +97,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - accUpdate(&accelerometerConfig()->accelerometerTrims); + accUpdate(&accelerometerConfigMutable()->accelerometerTrims); } static void taskHandleSerial(timeUs_t currentTimeUs) @@ -128,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + updateCurrentMeter(ibatTimeSinceLastServiced, rxConfig(), flight3DConfig()->deadband3d_throttle); } } } @@ -201,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs) telemetryCheckState(); if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + telemetryProcess(currentTimeUs, rxConfig(), flight3DConfig()->deadband3d_throttle); } } #endif diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index fea3bdf4b6..f13d105fb0 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -31,6 +31,8 @@ #include "common/maths.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -59,7 +61,7 @@ #include "flight/failsafe.h" -static motorConfig_t *motorConfig; +static const motorConfig_t *motorConfig; static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) @@ -115,7 +117,7 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) @@ -128,7 +130,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) +void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -318,12 +320,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat } -bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId) +bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId) { uint8_t index; for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; + const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { return true; @@ -647,7 +649,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) #define RESET_FREQUENCY_2HZ (1000 / 2) -void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig) +void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig) { uint8_t adjustmentIndex; uint32_t now = millis(); @@ -728,7 +730,7 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) { motorConfig = motorConfigToUse; pidProfile = pidProfileToUse; @@ -740,4 +742,3 @@ void resetAdjustmentStates(void) { memset(adjustmentStates, 0, sizeof(adjustmentStates)); } - diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index e416b682b1..4f2533afd1 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -19,6 +19,8 @@ #include +#include "config/parameter_group.h" + typedef enum { BOXARM = 0, BOXANGLE, @@ -140,6 +142,8 @@ typedef struct modeActivationCondition_s { channelRange_t range; } modeActivationCondition_t; +PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); + typedef struct modeActivationProfile_s { modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; } modeActivationProfile_t; @@ -158,6 +162,8 @@ typedef struct controlRateConfig_s { uint16_t tpa_breakpoint; // Breakpoint where TPA is activated } controlRateConfig_t; +//!!TODO PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); + extern int16_t rcCommand[4]; extern int16_t rcCommandSmooth[4]; @@ -169,18 +175,22 @@ typedef struct rcControlsConfig_s { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) } rcControlsConfig_t; +PG_DECLARE(rcControlsConfig_t, rcControlsConfig); + typedef struct armingConfig_s { uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 } armingConfig_t; +PG_DECLARE(armingConfig_t, armingConfig); + bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); struct rxConfig_s; -throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +throttleStatus_e calculateThrottleStatus(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -267,6 +277,8 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 15 +PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); + typedef struct adjustmentProfile_s { adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; } adjustmentProfile_t; @@ -274,12 +286,12 @@ typedef struct adjustmentProfile_s { bool isAirmodeActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig, const struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); -bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); +bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; struct motorConfig_s; -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 8f447cb2cb..84b437b221 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -34,7 +34,7 @@ #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t *motorConfig) +void generateThrottleCurve(controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig) { uint8_t i; uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle); diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index 41132a98f3..c13851969e 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -19,7 +19,7 @@ struct controlRateConfig_s; struct motorConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct motorConfig_s *motorConfig); +void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index c0603b4f01..f0d2a9e16f 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -24,6 +24,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "fc/config.h" @@ -50,9 +53,10 @@ static failsafeState_t failsafeState; -static failsafeConfig_t *failsafeConfig; - -static rxConfig_t *rxConfig; +#ifndef USE_PARAMETER_GROPUS +static const failsafeConfig_t *failsafeConfig; +static const rxConfig_t *rxConfig; +#endif static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC @@ -72,15 +76,23 @@ static void failsafeReset(void) /* * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. */ -void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse) +void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(failsafeConfigToUse); +#else failsafeConfig = failsafeConfigToUse; +#endif failsafeReset(); } -void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) +void failsafeInit(const rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) { +#ifdef USE_PARAMETER_GROUPS + (void)(intialRxConfig); +#else rxConfig = intialRxConfig; +#endif deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d166e1c360..be32b769e5 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + #define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5) #define MILLIS_PER_TENTH_SECOND 100 #define MILLIS_PER_SECOND 1000 @@ -36,6 +38,8 @@ typedef struct failsafeConfig_s { uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it } failsafeConfig_t; +PG_DECLARE(failsafeConfig_t, failsafeConfig); + typedef enum { FAILSAFE_IDLE = 0, FAILSAFE_RX_LOSS_DETECTED, @@ -71,8 +75,8 @@ typedef struct failsafeState_s { } failsafeState_t; struct rxConfig_s; -void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); -void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); +void failsafeInit(const struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); +void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse); void failsafeStartMonitoring(void); void failsafeUpdateState(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a25d992285..fadaf5e782 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -30,6 +30,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "sensors/sensors.h" diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9525c251ea..719a9f9ff5 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -21,6 +21,8 @@ #include "common/maths.h" #include "common/time.h" +#include "config/parameter_group.h" + #include "sensors/acceleration.h" // Exported symbols @@ -65,6 +67,8 @@ typedef struct imuConfig_s { accDeadband_t accDeadband; } imuConfig_t; +PG_DECLARE(imuConfig_t, imuConfig); + typedef struct imuRuntimeConfig_s { float dcm_ki; float dcm_kp; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index accc55505e..ddac5029db 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -28,6 +28,10 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/pwm_output.h" @@ -46,9 +50,6 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "config/feature.h" -#include "config/config_master.h" - #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 @@ -61,11 +62,13 @@ static float motorMixRange; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; -static mixerConfig_t *mixerConfig; -static flight3DConfig_t *flight3DConfig; -static motorConfig_t *motorConfig; static airplaneConfig_t *airplaneConfig; -rxConfig_t *rxConfig; +#ifndef USE_PARAMETER_GROUPS +static const mixerConfig_t *mixerConfig; +static const flight3DConfig_t *flight3DConfig; +static const motorConfig_t *motorConfig; +const rxConfig_t *rxConfig; +#endif mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -253,7 +256,7 @@ float getMotorMixRange() bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT - switch(motorConfig->motorPwmProtocol) { + switch(motorConfig()->motorPwmProtocol) { case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: @@ -273,25 +276,25 @@ void initEscEndpoints(void) { if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); else - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent); + motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; } else #endif { - disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand; - motorOutputLow = motorConfig->minthrottle; - motorOutputHigh = motorConfig->maxthrottle; - deadbandMotor3dHigh = flight3DConfig->deadband3d_high; - deadbandMotor3dLow = flight3DConfig->deadband3d_low; + disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; + motorOutputLow = motorConfig()->minthrottle; + motorOutputHigh = motorConfig()->maxthrottle; + deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; + deadbandMotor3dLow = flight3DConfig()->deadband3d_low; } - rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck); - rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle; - rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle; + rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); + rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle; + rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; } void mixerUseConfigs( @@ -301,11 +304,18 @@ void mixerUseConfigs( airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(flight3DConfigToUse); + (void)(motorConfigToUse); + (void)(mixerConfigToUse); + (void)(rxConfigToUse); +#else flight3DConfig = flight3DConfigToUse; motorConfig = motorConfigToUse; mixerConfig = mixerConfigToUse; - airplaneConfig = airplaneConfigToUse; rxConfig = rxConfigToUse; +#endif + airplaneConfig = airplaneConfigToUse; } void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) @@ -440,25 +450,25 @@ void mixTable(pidProfile_t *pidProfile) // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { - if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. + if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. - if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling + if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; - } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling + } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling motorOutputMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive + } else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive motorOutputMax = deadbandMotor3dLow; motorOutputMin = motorOutputLow; - throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle; + throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; currentThrottleInputRange = rcCommandThrottleRange3dLow; if(isMotorProtocolDshot()) mixerInversion = true; } else { // Deadband handling from positive to negative @@ -468,7 +478,7 @@ void mixTable(pidProfile_t *pidProfile) currentThrottleInputRange = rcCommandThrottleRange3dHigh; } } else { - throttle = rcCommand[THROTTLE] - rxConfig->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; currentThrottleInputRange = rcCommandThrottleRange; motorOutputMin = motorOutputLow; motorOutputMax = motorOutputHigh; @@ -493,7 +503,7 @@ void mixTable(pidProfile_t *pidProfile) motorMix[i] = scaledAxisPIDf[PITCH] * currentMixer[i].pitch + scaledAxisPIDf[ROLL] * currentMixer[i].roll + - scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction); + scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction); if (vbatCompensationFactor > 1.0f) { motorMix[i] *= vbatCompensationFactor; // Add voltage compensation @@ -541,7 +551,7 @@ void mixTable(pidProfile_t *pidProfile) // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { - if (((rcData[THROTTLE]) < rxConfig->mincheck)) { + if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { motor[i] = disarmMotorOutput; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 8a84ea8acb..6a206f4e86 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -17,6 +17,9 @@ #pragma once +#include "config/parameter_group.h" +#include "drivers/io_types.h" + #define MAX_SUPPORTED_MOTORS 12 #define QUAD_MOTOR_COUNT 4 @@ -85,6 +88,8 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; +PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer); + // Custom mixer configuration typedef struct mixer_s { uint8_t motorCount; @@ -97,6 +102,8 @@ typedef struct mixerConfig_s { int8_t yaw_motor_direction; } mixerConfig_t; +PG_DECLARE(mixerConfig_t, mixerConfig); + typedef struct flight3DConfig_s { uint16_t deadband3d_low; // min 3d value uint16_t deadband3d_high; // max 3d value @@ -104,6 +111,22 @@ typedef struct flight3DConfig_s { uint16_t deadband3d_throttle; // default throttle deadband from MIDRC } flight3DConfig_t; +PG_DECLARE(flight3DConfig_t, flight3DConfig); + +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) + 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]; +} motorConfig_t; + +PG_DECLARE(motorConfig_t, motorConfig); + typedef struct airplaneConfig_s { int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign } airplaneConfig_t; @@ -113,8 +136,6 @@ 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(); @@ -122,7 +143,7 @@ float getMotorMixRange(); void mixerUseConfigs( flight3DConfig_t *flight3DConfigToUse, - struct motorConfig_s *motorConfigToUse, + motorConfig_t *motorConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, struct rxConfig_s *rxConfigToUse); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index fc1c4a144d..0b0229c3de 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -31,6 +31,9 @@ #include "common/maths.h" #include "common/time.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "fc/config.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bcbdbaaaa9..1f855739f9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -28,6 +28,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/fc_core.h" #include "fc/fc_rc.h" diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 2c40241d34..d88a937a5b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,6 +19,8 @@ #include +#include "config/parameter_group.h" + #define PID_CONTROLLER_BETAFLIGHT 1 #define PID_MIXER_SCALING 1000.0f #define PID_SERVO_MIXER_SCALING 0.7f @@ -85,6 +87,8 @@ typedef struct pidProfile_s { float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms } pidProfile_t; +PG_DECLARE_PROFILE(pidProfile_t, pidProfile); + typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate } pidConfig_t; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 2ba83eb026..87a276fe16 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -28,6 +28,9 @@ #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/pwm_output.h" #include "drivers/system.h" @@ -47,7 +50,6 @@ #include "config/feature.h" extern mixerMode_e currentMixerMode; -extern rxConfig_t *rxConfig; static servoMixerConfig_t *servoMixerConfig; @@ -276,7 +278,7 @@ void writeServos(void) case MIXER_TRI: case MIXER_CUSTOM_TRI: - if (servoMixerConfig->tri_unarmed_servo) { + if (servoMixerConfig()->tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); } else { @@ -346,7 +348,7 @@ STATIC_UNIT_TESTED void servoMixer(void) input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { input[INPUT_STABILIZED_YAW] *= -1; } } @@ -362,14 +364,14 @@ STATIC_UNIT_TESTED void servoMixer(void) // 2000 - 1500 = +500 // 1500 - 1500 = 0 // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; - input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; - input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; - input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; - input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; - input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; - input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; + input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc; + input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc; + input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc; + input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc; + input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig()->midrc; + input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig()->midrc; + input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig()->midrc; + input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig()->midrc; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) servo[i] = 0; @@ -440,7 +442,7 @@ void servoTable(void) servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { + if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { @@ -471,10 +473,10 @@ void filterServos(void) uint32_t startTime = micros(); #endif - if (servoMixerConfig->servo_lowpass_enable) { + if (servoMixerConfig()->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime); + biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig()->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 43f0b19489..8c5b735413 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + #define MAX_SUPPORTED_SERVOS 8 // These must be consecutive, see 'reversedSources' @@ -87,6 +89,8 @@ typedef struct servoMixer_s { #define MAX_SERVO_SPEED UINT8_MAX #define MAX_SERVO_BOXES 3 +PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers); + // Custom mixer configuration typedef struct mixerRules_s { uint8_t servoRuleCount; @@ -94,6 +98,7 @@ typedef struct mixerRules_s { } mixerRules_t; typedef struct servoParam_s { + uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted int16_t min; // servo min int16_t max; // servo max int16_t middle; // servo middle @@ -101,8 +106,9 @@ typedef struct servoParam_s { uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted -} __attribute__ ((__packed__)) servoParam_t; +} servoParam_t; + +PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams); typedef struct servoMixerConfig_s{ uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed @@ -110,6 +116,8 @@ typedef struct servoMixerConfig_s{ int8_t servo_lowpass_enable; // enable/disable lowpass filter } servoMixerConfig_t; +//!!TODO PG_DECLARE(servoConfig_t, servoConfig); + typedef struct servoProfile_s { servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; } servoProfile_t; diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index 79e1815dae..cb015b0987 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -24,7 +24,8 @@ #include "common/utils.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/display.h" #include "drivers/max7456.h" diff --git a/src/main/io/gimbal.h b/src/main/io/gimbal.h index 914ce05ba5..969242fee3 100644 --- a/src/main/io/gimbal.h +++ b/src/main/io/gimbal.h @@ -17,6 +17,8 @@ #pragma once +#include "config/parameter_group.h" + typedef enum { GIMBAL_MODE_NORMAL = 0, GIMBAL_MODE_MIXTILT = 1 @@ -27,3 +29,5 @@ typedef enum { typedef struct gimbalConfig_s { uint8_t mode; } gimbalConfig_t; + +PG_DECLARE(gimbalConfig_t, gimbalConfig); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 9cae15f265..75f5e3c84d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -35,6 +35,11 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/system.h" +#include "drivers/light_led.h" #include "drivers/light_led.h" #include "drivers/system.h" diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 484f7bf52a..f776de69e2 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -19,6 +19,8 @@ #include "common/time.h" +#include "config/parameter_group.h" + #define LAT 0 #define LON 1 @@ -68,6 +70,8 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; } gpsConfig_t; +PG_DECLARE(gpsConfig_t, gpsConfig); + typedef struct gpsCoordinateDDDMMmmmm_s { int16_t dddmm; int16_t mmmm; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 31734e9fd3..5b0074c9d5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -31,6 +31,9 @@ #include "common/maths.h" #include "common/typeconversion.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/light_ws2811strip.h" #include "drivers/system.h" #include "drivers/serial.h" @@ -44,6 +47,11 @@ #include "common/axis.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -73,10 +81,6 @@ #include "telemetry/telemetry.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - /* PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 4362a3d555..f90f935792 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -19,6 +19,7 @@ #include "common/color.h" #include "common/time.h" +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_MAX_STRIP_LENGTH 32 @@ -147,6 +148,8 @@ typedef struct ledStripConfig_s { ioTag_t ioTag; } ledStripConfig_t; +PG_DECLARE(ledStripConfig_t, ledStripConfig); + ledConfig_t *ledConfigs; hsvColor_t *colors; modeColorIndexes_t *modeColors; diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 82517b2e9d..817e950356 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -20,14 +20,4 @@ #include "drivers/io_types.h" #include "flight/mixer.h" -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 - 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]; -} motorConfig_t; + diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 206f1a1e5d..f31e4d72e4 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -40,6 +40,11 @@ #include "common/printf.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/max7456_symbols.h" #include "drivers/display.h" #include "drivers/system.h" @@ -51,19 +56,15 @@ #include "cms/cms_types.h" #include "cms/cms_menu_osd.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/osd.h" #include "io/vtx.h" -#include "io/asyncfatfs/asyncfatfs.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.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 diff --git a/src/main/io/osd.h b/src/main/io/osd.h index c6539b42d7..00b6abc163 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define VISIBLE_FLAG 0x0800 #define VISIBLE(x) (x & VISIBLE_FLAG) @@ -67,6 +68,9 @@ typedef struct osd_profile_s { osd_unit_e units; } osd_profile_t; +// !!TODO change to osdConfig_t +PG_DECLARE(osd_profile_t, osdConfig); + struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); void osdResetConfig(osd_profile_t *osdProfile); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 6c96238aae..d79d135a54 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 5a68a18041..cfed31388c 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/serial.h" typedef enum { @@ -111,6 +112,8 @@ typedef struct serialConfig_s { uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. } serialConfig_t; +PG_DECLARE(serialConfig_t, serialConfig); + typedef void serialConsumer(uint8_t); // diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 711ceec755..804b77b0b0 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -26,3 +26,5 @@ typedef struct servoConfig_s { uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz) ioTag_t ioTags[MAX_SUPPORTED_SERVOS]; } servoConfig_t; + +PG_DECLARE(servoConfig_t, servoConfig); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 8db4c4279b..eb44e3f0b4 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -26,10 +26,14 @@ #include "io/osd.h" //External dependencies -#include "config/config_master.h" #include "config/config_eeprom.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/vtx_rtc6705.h" + #include "fc/runtime_config.h" + #include "io/beeper.h" diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index e87db2694d..e6c726f503 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -19,32 +19,36 @@ #include #include +#include #include #include "platform.h" #if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) +#include "build/build_config.h" + #include "cms/cms.h" #include "cms/cms_types.h" -#include "string.h" #include "common/printf.h" #include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/vtx_common.h" -#include "io/serial.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx_string.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "flight/pid.h" -#include "config/config_master.h" -#include "build/build_config.h" +#include "io/serial.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx_string.h" //#define SMARTAUDIO_DPRINTF //#define SMARTAUDIO_DEBUG_MONITOR diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 897e8bc0bd..adf8939f7c 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -30,6 +30,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/rx_pwm.h" @@ -90,7 +92,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) rxRuntimeConfig_t rxRuntimeConfig; -static const rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) @@ -108,7 +109,7 @@ static uint8_t nullFrameStatus(void) void useRxConfig(const rxConfig_t *rxConfigToUse) { - rxConfig = rxConfigToUse; + (void)(rxConfigToUse); } #define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels @@ -126,8 +127,8 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void) STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration) { - return pulseDuration >= rxConfig->rx_min_usec && - pulseDuration <= rxConfig->rx_max_usec; + return pulseDuration >= rxConfig()->rx_min_usec && + pulseDuration <= rxConfig()->rx_max_usec; } // pulse duration is in micro seconds (usec) @@ -204,20 +205,20 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } #endif -void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions) +void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions) { - useRxConfig(rxConfig); + useRxConfig(initialRxConfig); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rcSampleIndex = 0; needRxSignalMaxDelayUs = DELAY_10_HZ; for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcData[i] = rxConfig->midrc; + rcData[i] = rxConfig()->midrc; rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec; + rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -237,7 +238,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { - const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig); + const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SERIAL); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -248,14 +249,14 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #ifdef USE_RX_MSP if (feature(FEATURE_RX_MSP)) { - rxMspInit(rxConfig, &rxRuntimeConfig); + rxMspInit(rxConfig(), &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifdef USE_RX_SPI if (feature(FEATURE_RX_SPI)) { - const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig); + const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SPI); rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -266,7 +267,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #if defined(USE_PWM) || defined(USE_PPM) if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { - rxPwmInit(rxConfig, &rxRuntimeConfig); + rxPwmInit(rxConfig(), &rxRuntimeConfig); } #endif } @@ -376,7 +377,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static uint16_t getRxfailValue(uint8_t channel) { - const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; + const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig()->failsafe_channel_configurations[channel]; switch(channelFailsafeConfiguration->mode) { case RX_FAILSAFE_MODE_AUTO: @@ -384,12 +385,12 @@ static uint16_t getRxfailValue(uint8_t channel) case ROLL: case PITCH: case YAW: - return rxConfig->midrc; + return rxConfig()->midrc; case THROTTLE: if (feature(FEATURE_3D)) - return rxConfig->midrc; + return rxConfig()->midrc; else - return rxConfig->rx_min_usec; + return rxConfig()->rx_min_usec; } /* no break */ @@ -420,7 +421,7 @@ static uint8_t getRxChannelCount(void) { static uint8_t maxChannelsAllowed; if (!maxChannelsAllowed) { - uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT; + uint8_t maxChannels = rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT; if (maxChannels > rxRuntimeConfig.channelCount) { maxChannelsAllowed = rxRuntimeConfig.channelCount; } else { @@ -436,14 +437,14 @@ static void readRxChannelsApplyRanges(void) const int channelCount = getRxChannelCount(); for (int channel = 0; channel < channelCount; channel++) { - const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel); // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { - sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]); + sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]); } rcRaw[channel] = sample; @@ -548,10 +549,10 @@ static void updateRSSIPWM(void) { int16_t pwmRssi = 0; // Read value of AUX channel as rssi - pwmRssi = rcData[rxConfig->rssi_channel - 1]; + pwmRssi = rcData[rxConfig()->rssi_channel - 1]; // RSSI_Invert option - if (rxConfig->rssi_ppm_invert) { + if (rxConfig()->rssi_ppm_invert) { pwmRssi = ((2000 - pwmRssi) + 1000); } @@ -578,7 +579,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) int16_t adcRssiMean = 0; uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale; + uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; @@ -599,7 +600,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) void updateRSSI(timeUs_t currentTimeUs) { - if (rxConfig->rssi_channel > 0) { + if (rxConfig()->rssi_channel > 0) { updateRSSIPWM(); } else if (feature(FEATURE_RSSI_ADC)) { updateRSSIADC(currentTimeUs); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 484d65914a..17f8d27763 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -18,6 +18,7 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" #define STICK_CHANNEL_COUNT 4 @@ -105,11 +106,17 @@ typedef struct rxFailsafeChannelConfiguration_s { uint8_t step; } rxFailsafeChannelConfiguration_t; +//!!TODO change to rxFailsafeChannelConfig_t +PG_DECLARE_ARRAY(rxFailsafeChannelConfiguration_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs); + typedef struct rxChannelRangeConfiguration_s { uint16_t min; uint16_t max; } rxChannelRangeConfiguration_t; +//!!TODO change to rxChannelRangeConfig_t +PG_DECLARE_ARRAY(rxChannelRangeConfiguration_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs); + typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. @@ -139,6 +146,8 @@ typedef struct rxConfig_s { rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT]; } rxConfig_t; +PG_DECLARE(rxConfig_t, rxConfig); + #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) struct rxRuntimeConfig_s; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index ded23de3cf..d8395f141e 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -27,6 +27,9 @@ #include "common/axis.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index fda259e940..70905bb3a9 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" @@ -66,6 +67,8 @@ typedef struct accelerometerConfig_s { rollAndPitchTrims_t accelerometerTrims; } accelerometerConfig_t; +PG_DECLARE(accelerometerConfig_t, accelerometerConfig); + bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bac86ad16c..42a013259b 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -23,6 +23,9 @@ #include "common/maths.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/barometer.h" #include "drivers/barometer_bmp085.h" #include "drivers/barometer_bmp280.h" @@ -51,7 +54,9 @@ static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; +#ifndef USE_PARAMETER_GROUPS static const barometerConfig_t *barometerConfig; +#endif bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { @@ -121,7 +126,11 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) void useBarometerConfig(const barometerConfig_t *barometerConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(barometerConfigToUse); +#else barometerConfig = barometerConfigToUse; +#endif } bool isBaroCalibrationComplete(void) @@ -160,7 +169,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading) return newPressureReading; } -#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1) +#define PRESSURE_SAMPLE_COUNT (barometerConfig()->baro_sample_count - 1) static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) { @@ -213,7 +222,7 @@ uint32_t baroUpdate(void) baro.dev.get_up(); baro.dev.start_ut(); baro.dev.calculate(&baroPressure, &baroTemperature); - baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); + baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure); state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; break; @@ -228,7 +237,7 @@ int32_t baroCalculateAltitude(void) // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise + baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise return baro.BaroAlt; } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 536e6c453a..1f6e6687fa 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/barometer.h" typedef enum { @@ -37,6 +38,8 @@ typedef struct barometerConfig_s { float baro_cf_alt; // apply CF to use ACC for height estimation } barometerConfig_t; +PG_DECLARE(barometerConfig_t, barometerConfig); + typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index f79e581d2e..c3f6993f76 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -25,6 +25,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/adc.h" #include "drivers/system.h" @@ -65,6 +68,10 @@ int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start +#ifndef USE_PARAMETER_GROUPS +batteryConfig_t *batteryConfig; +#endif + static batteryState_e vBatState; static batteryState_e consumptionState; @@ -72,7 +79,7 @@ static uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier); + return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier); } static void updateBatteryVoltage(void) @@ -86,7 +93,7 @@ static void updateBatteryVoltage(void) } #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { + if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) { escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0; if (debugMode == DEBUG_BATTERY) { @@ -134,20 +141,20 @@ void updateBattery(void) uint16_t vBatMeasured = vbatLatest; /* battery has just been connected*/ - if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { + if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ vBatState = BATTERY_OK; - unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1; + unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1; if (cells > 8) { // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) cells = 8; } batteryCellCount = cells; - batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; - batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */ - } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { + batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; + batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */ + } else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) { vBatState = BATTERY_NOT_PRESENT; batteryCellCount = 0; batteryWarningVoltage = 0; @@ -159,16 +166,16 @@ void updateBattery(void) debug[3] = batteryCellCount; } - if (batteryConfig->useVBatAlerts) { + if (batteryConfig()->useVBatAlerts) { switch(vBatState) { case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_WARNING; } break; case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { + if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) { vBatState = BATTERY_CRITICAL; } else if (vbat > batteryWarningVoltage) { vBatState = BATTERY_OK; @@ -208,7 +215,11 @@ const char * getBatteryStateString(void) void batteryInit(batteryConfig_t *initialBatteryConfig) { +#ifndef USE_PARAMETER_GROUPS + (void)initialBatteryConfig; +#else batteryConfig = initialBatteryConfig; +#endif vBatState = BATTERY_NOT_PRESENT; consumptionState = BATTERY_OK; batteryCellCount = 0; @@ -221,9 +232,9 @@ static int32_t currentSensorToCentiamps(uint16_t src) int32_t millivolts; millivolts = ((uint32_t)src * ADCVREF) / 4096; - millivolts -= batteryConfig->currentMeterOffset; + millivolts -= batteryConfig()->currentMeterOffset; - return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps + return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps } static void updateBatteryCurrent(void) @@ -251,10 +262,10 @@ static void updateCurrentDrawn(int32_t lastUpdateAt) void updateConsumptionWarning(void) { - if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { + if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) { if (calculateBatteryPercentage() == 0) { vBatState = BATTERY_CRITICAL; - } else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) { + } else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) { consumptionState = BATTERY_WARNING; } else { consumptionState = BATTERY_OK; @@ -264,35 +275,30 @@ void updateConsumptionWarning(void) } } -void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { + (void)(rxConfig); if (getBatteryState() != BATTERY_NOT_PRESENT) { - switch(batteryConfig->currentMeterType) { + switch(batteryConfig()->currentMeterType) { case CURRENT_SENSOR_ADC: updateBatteryCurrent(); - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_VIRTUAL: - amperageLatest = (int32_t)batteryConfig->currentMeterOffset; + amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000; } amperage = amperageLatest; - updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR @@ -328,7 +334,7 @@ float calculateVbatPidCompensation(void) { float batteryScaler = 1.0f; if (feature(FEATURE_VBAT) && batteryCellCount > 0) { // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference - batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); + batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f); } return batteryScaler; } @@ -337,11 +343,11 @@ uint8_t calculateBatteryPercentage(void) { uint8_t batteryPercentage = 0; if (batteryCellCount > 0) { - uint16_t batteryCapacity = batteryConfig->batteryCapacity; + uint16_t batteryCapacity = batteryConfig()->batteryCapacity; if (batteryCapacity > 0) { batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100); } else { - batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100); } } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3eb68940c8..69be1bf98d 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "common/maths.h" // for fix12_t #ifndef VBAT_SCALE_DEFAULT @@ -63,6 +64,8 @@ typedef struct batteryConfig_s { uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning } batteryConfig_t; +PG_DECLARE(batteryConfig_t, batteryConfig); + typedef enum { BATTERY_OK = 0, BATTERY_WARNING, @@ -77,15 +80,17 @@ extern uint16_t batteryWarningVoltage; extern int32_t amperageLatest; extern int32_t amperage; extern int32_t mAhDrawn; +#ifndef USE_PARAMETER_GROUPS +extern batteryConfig_t *batteryConfig; +#endif batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); void batteryInit(batteryConfig_t *initialBatteryConfig); -batteryConfig_t *batteryConfig; struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index c4f1988274..0b45b8ffe8 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -23,6 +23,9 @@ #include "common/maths.h" #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "boardalignment.h" diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index b1ab74cdf9..8c224f24e2 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -17,11 +17,15 @@ #pragma once +#include "config/parameter_group.h" + typedef struct boardAlignment_s { int32_t rollDegrees; int32_t pitchDegrees; int32_t yawDegrees; } boardAlignment_t; +PG_DECLARE(boardAlignment_t, boardAlignment); + void alignSensors(int32_t *dest, uint8_t rotation); void initBoardAlignment(const boardAlignment_t *boardAlignment); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 9cc1e29fc3..be53d8c9b6 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -22,6 +22,9 @@ #include "common/axis.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/compass.h" #include "drivers/compass_ak8975.h" #include "drivers/compass_ak8963.h" diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 4e534fea08..59b997d76f 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -17,8 +17,8 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/compass.h" - #include "sensors/sensors.h" @@ -47,6 +47,8 @@ typedef struct compassConfig_s { flightDynamicsTrims_t magZero; } compassConfig_t; +PG_DECLARE(compassConfig_t, compassConfig); + bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); void compassInit(const compassConfig_t *compassConfig); union flightDynamicsTrims_u; diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index aad7b1ea4a..4fe0876ba1 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -21,26 +21,28 @@ #include -#include "fc/config.h" +#include "build/debug.h" + #include "config/feature.h" -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/utils.h" -#include "drivers/system.h" +#include "drivers/pwm_output.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "drivers/pwm_output.h" +#include "drivers/system.h" -#include "io/serial.h" +#include "esc_sensor.h" + +#include "fc/config.h" #include "flight/mixer.h" #include "sensors/battery.h" -#include "build/debug.h" - -#include "esc_sensor.h" +#include "io/serial.h" /* KISS ESC TELEMETRY PROTOCOL diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d0b15ece6b..1fb1f66181 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,6 +28,9 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/accgyro.h" #include "drivers/accgyro_adxl345.h" #include "drivers/accgyro_bma280.h" @@ -69,7 +72,9 @@ gyro_t gyro; // gyro access functions static int32_t gyroADC[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; +#ifndef USE_PARAMETER_GROUPS static const gyroConfig_t *gyroConfig; +#endif static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -233,7 +238,11 @@ static bool gyroDetect(gyroDev_t *dev) bool gyroInit(const gyroConfig_t *gyroConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + (void)(gyroConfigToUse); +#else gyroConfig = gyroConfigToUse; +#endif memset(&gyro, 0, sizeof(gyro)); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); @@ -261,8 +270,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) } // Must set gyro sample rate before initialisation - gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz); - gyro.dev.lpf = gyroConfig->gyro_lpf; + gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz); + gyro.dev.lpf = gyroConfig()->gyro_lpf; gyro.dev.init(&gyro.dev); gyroInitFilters(); return true; @@ -280,43 +289,43 @@ void gyroInitFilters(void) notchFilter1ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply; - if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known - if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { + if (gyroConfig()->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known + if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } - } else if (gyroConfig->gyro_soft_lpf_type == FILTER_PT1) { + } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt); + pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); + firFilterDenoiseInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime); } } } - if (gyroConfig->gyro_soft_notch_hz_1) { + if (gyroConfig()->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); + const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(notchFilter1[axis], gyroConfig()->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } - if (gyroConfig->gyro_soft_notch_hz_2) { + if (gyroConfig()->gyro_soft_notch_hz_2) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; - const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); + const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } @@ -441,7 +450,7 @@ void gyroUpdate(void) if (calibrationComplete) { #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL) // SPI-based gyro so can read and update in ISR - if (gyroConfig->gyro_isr_update) { + if (gyroConfig()->gyro_isr_update) { mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR); return; } @@ -450,7 +459,7 @@ void gyroUpdate(void) debug[3] = (uint16_t)(micros() & 0xffff); #endif } else { - performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); + performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold); } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 1fdf8820a6..5bd68fcb91 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/accgyro.h" #include "common/axis.h" @@ -59,6 +60,8 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_2; } gyroConfig_t; +PG_DECLARE(gyroConfig_t, gyroConfig); + void gyroSetCalibrationCycles(void); bool gyroInit(const gyroConfig_t *gyroConfigToUse); void gyroInitFilters(void); diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index e8c78c6087..0e3d0768a0 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -29,6 +29,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/runtime_config.h" diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 16151fc2f9..d123679394 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,10 +17,9 @@ #pragma once +#include "config/parameter_group.h" #include "common/time.h" - #include "drivers/sonar_hcsr04.h" - #include "sensors/battery.h" #define SONAR_OUT_OF_RANGE (-1) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 18717103cd..4912bc15f8 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -77,8 +77,6 @@ #include "bus_bst.h" #include "i2c_bst.h" -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse); - #define BST_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made @@ -640,13 +638,13 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_SERVO_MIX_RULES: for (i = 0; i < MAX_SERVO_RULES; i++) { - bstWrite8(customServoMixer(i)->targetChannel); - bstWrite8(customServoMixer(i)->inputSource); - bstWrite8(customServoMixer(i)->rate); - bstWrite8(customServoMixer(i)->speed); - bstWrite8(customServoMixer(i)->min); - bstWrite8(customServoMixer(i)->max); - bstWrite8(customServoMixer(i)->box); + bstWrite8(customServoMixers(i)->targetChannel); + bstWrite8(customServoMixers(i)->inputSource); + bstWrite8(customServoMixers(i)->rate); + bstWrite8(customServoMixers(i)->speed); + bstWrite8(customServoMixers(i)->min); + bstWrite8(customServoMixers(i)->max); + bstWrite8(customServoMixers(i)->box); } break; #endif @@ -1179,13 +1177,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (i >= MAX_SERVO_RULES) { ret = BST_FAILED; } else { - customServoMixer(i)->targetChannel = bstRead8(); - customServoMixer(i)->inputSource = bstRead8(); - customServoMixer(i)->rate = bstRead8(); - customServoMixer(i)->speed = bstRead8(); - customServoMixer(i)->min = bstRead8(); - customServoMixer(i)->max = bstRead8(); - customServoMixer(i)->box = bstRead8(); + customServoMixers(i)->targetChannel = bstRead8(); + customServoMixers(i)->inputSource = bstRead8(); + customServoMixers(i)->rate = bstRead8(); + customServoMixers(i)->speed = bstRead8(); + customServoMixers(i)->min = bstRead8(); + customServoMixers(i)->max = bstRead8(); + customServoMixers(i)->box = bstRead8(); loadCustomServoMixer(); } #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index a9270aa8af..313a73b635 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,6 +17,7 @@ #pragma once +//#define USE_PARAMETER_GROUPS // type conversion warnings. // -Wconversion can be turned on to enable the process of eliminating these warnings //#pragma GCC diagnostic warning "-Wconversion" diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d62326e113..baa00554a5 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -32,10 +32,8 @@ #define CLEANFLIGHT #endif -#ifdef CLEANFLIGHT #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#endif #include "common/streambuf.h" #include "common/utils.h" @@ -184,7 +182,7 @@ void crsfFrameBatterySensor(sbuf_t *dst) const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); #else crsfSerialize16(dst, amperage / 10); - const uint32_t batteryCapacity = batteryConfig->batteryCapacity; + const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); #endif crsfSerialize8(dst, (batteryCapacity >> 16)); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index f696419016..3dad42c3fc 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -33,6 +33,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -72,13 +74,13 @@ static serialPortConfig_t *portConfig; #define FRSKY_BAUDRATE 9600 #define FRSKY_INITIAL_PORT_MODE MODE_TX -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool frskyTelemetryEnabled = false; static portSharing_e frskyPortSharing; -extern batteryConfig_t *batteryConfig; - #define CYCLETIME 125 #define PROTOCOL_HEADER 0x5E @@ -196,7 +198,7 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { sendDataHead(ID_RPM); #ifdef USE_ESC_SENSOR @@ -213,7 +215,7 @@ static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadba throttleForRPM = 0; serialize16(throttleForRPM); } else { - serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER)); + serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER)); } #endif } @@ -240,7 +242,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void) } sendDataHead(ID_TEMPRATURE2); - if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) { + if (telemetryConfig()->frsky_unit == FRSKY_UNIT_METRICS) { serialize16(satellite); } else { float tmp = (satellite - 32) / 1.8f; @@ -286,7 +288,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left - if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) { + if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { result->dddmm = deg * 100 + min; } else { result->dddmm = deg * 60 + min; @@ -332,8 +334,8 @@ static void sendFakeLatLong(void) // Heading is only displayed on OpenTX if non-zero lat/long is also sent int32_t coord[2] = {0,0}; - coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); + coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); + coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); sendLatLong(coord); } @@ -409,7 +411,7 @@ static void sendVoltage(void) */ static void sendVoltageAmp(void) { - if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { + if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { /* * Use new ID 0x39 to send voltage directly in 0.1 volts resolution */ @@ -418,7 +420,7 @@ static void sendVoltageAmp(void) } else { uint16_t voltage = (getVbat() * 110) / 21; uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = voltage / batteryCellCount; } else { vfasVoltage = voltage; @@ -440,7 +442,7 @@ static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); - if (batteryConfig->batteryCapacity > 0) { + if (batteryConfig()->batteryCapacity > 0) { serialize16((uint16_t)calculateBatteryPercentage()); } else { serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); @@ -455,9 +457,13 @@ static void sendHeading(void) serialize16(0); } -void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initFrSkyTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); } @@ -475,7 +481,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!frskyPort) { return; } @@ -509,7 +515,7 @@ void checkFrSkyTelemetryState(void) } } -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void handleFrSkyTelemetry(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { if (!frskyTelemetryEnabled) { return; diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 89ba617c8f..419b4b407b 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -23,11 +23,11 @@ typedef enum { } frskyVFasPrecision_e; struct rxConfig_s; -void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void handleFrSkyTelemetry(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); void checkFrSkyTelemetryState(void); struct telemetryConfig_s; -void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig); +void initFrSkyTelemetry(const struct telemetryConfig_s *telemetryConfig); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index c43822b2a3..dacf2393f1 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -65,6 +65,10 @@ #include "common/axis.h" #include "common/time.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "drivers/system.h" @@ -108,7 +112,9 @@ static uint8_t hottMsgCrc; static serialPort_t *hottPort = NULL; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool hottTelemetryEnabled = false; static portSharing_e hottPortSharing; @@ -219,7 +225,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) static bool shouldTriggerBatteryAlarmNow(void) { - return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); + return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND)); } static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage) @@ -289,9 +295,13 @@ void freeHoTTTelemetryPort(void) hottTelemetryEnabled = false; } -void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initHoTTTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 5370dc24f3..bd437afa93 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -490,7 +490,7 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(timeUs_t currentTimeUs); void checkHoTTTelemetryState(void); -void initHoTTTelemetry(telemetryConfig_t *telemetryConfig); +void initHoTTTelemetry(const telemetryConfig_t *telemetryConfig); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 8e6341e90b..9bb193454c 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -32,7 +32,8 @@ #if defined(TELEMETRY) && defined(TELEMETRY_IBUS) -#include "config/config_master.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "common/axis.h" diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h index 8bbd30c5d8..3688ea2ffc 100644 --- a/src/main/telemetry/ibus.h +++ b/src/main/telemetry/ibus.h @@ -17,13 +17,11 @@ #pragma once -/* typedef struct ibusTelemetryConfig_s { uint8_t report_cell_voltage; // report vbatt divided with cellcount } ibusTelemetryConfig_t; PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig); -*/ void initIbusTelemetry(void); @@ -31,4 +29,4 @@ void handleIbusTelemetry(void); bool checkIbusTelemetryState(void); void configureIbusTelemetryPort(void); -void freeIbusTelemetryPort(void); \ No newline at end of file +void freeIbusTelemetryPort(void); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 038bbf497c..ab5a237db1 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -40,6 +40,7 @@ #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/utils.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -80,7 +81,9 @@ static serialPort_t *ltmPort; static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool ltmEnabled; static portSharing_e ltmPortSharing; static uint8_t ltm_crc; @@ -268,9 +271,13 @@ void freeLtmTelemetryPort(void) ltmEnabled = false; } -void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initLtmTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM); ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM); } diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index e5b4790c5a..e0603707da 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -20,7 +20,7 @@ #pragma once struct telemetryConfig_s; -void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initLtmTelemetry(const struct telemetryConfig_s *initialTelemetryConfig); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 7dbf92a1e2..5423919613 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -32,26 +32,18 @@ #include "common/axis.h" #include "common/color.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "fc/config.h" #include "fc/rc_controls.h" - -#include "io/serial.h" -#include "io/gimbal.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/motors.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" -#include "sensors/gyro.h" -#include "sensors/barometer.h" -#include "sensors/boardalignment.h" -#include "sensors/battery.h" - -#include "rx/rx.h" +#include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -60,16 +52,24 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + #include "telemetry/telemetry.h" #include "telemetry/mavlink.h" -#include "fc/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - -#include "fc/runtime_config.h" - // mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used // until this is resolved in mavlink library - ignore -Wpedantic for mavlink code #pragma GCC diagnostic push diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5b047861e6..13e09f4c6c 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -16,6 +16,11 @@ #include "common/maths.h" #include "common/utils.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -52,9 +57,6 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "config/config_profile.h" -#include "config/feature.h" - #include "msp/msp.h" extern profile_t *currentProfile; @@ -148,7 +150,9 @@ const uint16_t frSkyDataIdTable[] = { static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port. static serialPortConfig_t *portConfig; -static telemetryConfig_t *telemetryConfig; +#ifndef USE_PARAMETER_GROUPS +static const telemetryConfig_t *telemetryConfig; +#endif static bool smartPortTelemetryEnabled = false; static portSharing_e smartPortPortSharing; @@ -302,9 +306,13 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortSendPackageEx(FSSP_DATA_FRAME,payload); } -void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initSmartPortTelemetry(const telemetryConfig_t *initialTelemetryConfig) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(initialTelemetryConfig); +#else telemetryConfig = initialTelemetryConfig; +#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT); smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT); } @@ -326,11 +334,11 @@ void configureSmartPortTelemetryPort(void) portOptions_t portOptions = 0; - if (telemetryConfig->sportHalfDuplex) { + if (telemetryConfig()->sportHalfDuplex) { portOptions |= SERIAL_BIDIR; } - if (telemetryConfig->telemetry_inversion) { + if (telemetryConfig()->telemetry_inversion) { portOptions |= SERIAL_INVERTED; } @@ -622,7 +630,7 @@ void handleSmartPortTelemetry(void) case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT) && batteryCellCount > 0) { uint16_t vfasVoltage; - if (telemetryConfig->frsky_vfas_cell_voltage) { + if (telemetryConfig()->frsky_vfas_cell_voltage) { vfasVoltage = getVbat() / batteryCellCount; } else { vfasVoltage = getVbat(); @@ -752,7 +760,7 @@ void handleSmartPortTelemetry(void) } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; - } else if (telemetryConfig->pidValuesAsTelemetry){ + } else if (telemetryConfig()->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index d50c1fd575..e7d7722b2c 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,7 +7,7 @@ #pragma once -void initSmartPortTelemetry(telemetryConfig_t *); +void initSmartPortTelemetry(const telemetryConfig_t *); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 928acdc2b7..0d635d793b 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -25,6 +25,9 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" @@ -50,29 +53,35 @@ #include "telemetry/srxl.h" #include "telemetry/ibus.h" +#ifndef USE_PARAMETER_GROUPS static telemetryConfig_t *telemetryConfig; +#endif void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) { +#ifdef USE_PARAMETER_GROUPS + UNUSED(telemetryConfigToUse); +#else telemetryConfig = telemetryConfigToUse; +#endif } void telemetryInit(void) { #ifdef TELEMETRY_FRSKY - initFrSkyTelemetry(telemetryConfig); + initFrSkyTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_HOTT - initHoTTTelemetry(telemetryConfig); + initHoTTTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_SMARTPORT - initSmartPortTelemetry(telemetryConfig); + initSmartPortTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_LTM - initLtmTelemetry(telemetryConfig); + initLtmTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_JETIEXBUS - initJetiExBusTelemetry(telemetryConfig); + initJetiExBusTelemetry(telemetryConfig()); #endif #ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); @@ -95,7 +104,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) bool enabled = portSharing == PORTSHARING_NOT_SHARED; if (portSharing == PORTSHARING_SHARED) { - if (telemetryConfig->telemetry_switch) + if (telemetryConfig()->telemetry_switch) enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY); else enabled = ARMING_FLAG(ARMED); @@ -142,7 +151,7 @@ void telemetryCheckState(void) #endif } -void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void telemetryProcess(uint32_t currentTime, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { #ifdef TELEMETRY_FRSKY handleFrSkyTelemetry(rxConfig, deadband3d_throttle); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index aa60912147..34f59f2711 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -24,6 +24,9 @@ #pragma once +#include "config/parameter_group.h" +#include "io/serial.h" + typedef enum { FRSKY_FORMAT_DMS = 0, FRSKY_FORMAT_NMEA @@ -49,13 +52,15 @@ typedef struct telemetryConfig_s { uint8_t report_cell_voltage; } telemetryConfig_t; +PG_DECLARE(telemetryConfig_t, telemetryConfig); + void telemetryInit(void); bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); extern serialPort_t *telemetrySharedPort; void telemetryCheckState(void); struct rxConfig_s; -void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void telemetryProcess(uint32_t currentTime, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); bool telemetryDetermineEnabledState(portSharing_e portSharing); diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index b839912c00..a6b038441a 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -26,6 +26,7 @@ #define BARO extern "C" { + #include "platform.h"" #include "target.h" #include "drivers/display.h" #include "cms/cms.h" diff --git a/src/test/unit/parameter_groups_unittest.cc b/src/test/unit/parameter_groups_unittest.cc index a48e98e055..82c857e38a 100644 --- a/src/test/unit/parameter_groups_unittest.cc +++ b/src/test/unit/parameter_groups_unittest.cc @@ -29,7 +29,7 @@ extern "C" { #include "io/motors.h" -PG_DECLARE(motorConfig_t, motorConfig); +//PG_DECLARE(motorConfig_t, motorConfig); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index c5c400aa85..6b5f77acff 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -17,6 +17,8 @@ #pragma once +#define USE_PARAMETER_GROUPS + #define U_ID_0 0 #define U_ID_1 1 #define U_ID_2 2 @@ -30,8 +32,6 @@ #define USE_SERVOS #define TRANSPONDER -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - typedef enum { Mode_TEST = 0x0, diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index cfe99b7943..a5c4b8faa4 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -22,10 +22,10 @@ #include extern "C" { - #include "build/debug.h" - #include + #include "build/debug.h" + #include "common/axis.h" #include "common/filter.h" #include "common/gps_conversion.h" @@ -56,6 +56,8 @@ extern "C" { bool airMode; uint16_t vbat; serialPort_t *telemetrySharedPort; + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" @@ -135,10 +137,7 @@ TEST(TelemetryCrsfTest, TestGPS) TEST(TelemetryCrsfTest, TestBattery) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - batteryConfig_t workingBatteryConfig; - batteryConfig = &workingBatteryConfig; - memset(batteryConfig, 0, sizeof(batteryConfig_t)); vbat = 0; // 0.1V units int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); @@ -157,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery) vbat = 33; // 3.3V = 3300 mv amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps - batteryConfig->batteryCapacity = 1234; + batteryConfigMutable()->batteryCapacity = 1234; frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); voltage = frame[3] << 8 | frame[4]; // mV * 100 EXPECT_EQ(33, voltage); diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index bc0d60ad01..54caa75e99 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -29,6 +29,10 @@ extern "C" { #include "common/axis.h" #include "common/gps_conversion.h" + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/system.h" @@ -45,6 +49,8 @@ extern "C" { #include "telemetry/telemetry.h" #include "telemetry/hott.h" + + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); } #include "unittest_macros.h" From e41d6a3b5b7accaf8920c7115319400beb0f6f23 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 1 Feb 2017 10:00:41 +0000 Subject: [PATCH 84/97] Preparation for conversion to parameter groups --- src/main/fc/config.c | 8 +++---- src/main/fc/fc_msp.c | 8 +++---- src/main/flight/imu.h | 2 ++ src/main/io/serial.c | 29 ++++++++++++------------ src/main/io/serial.h | 11 +++++---- src/main/sensors/battery.c | 4 ++-- src/main/sensors/battery.h | 4 ++-- src/test/unit/cms_unittest.cc | 2 +- src/test/unit/target.h | 2 -- src/test/unit/telemetry_crsf_unittest.cc | 2 +- src/test/unit/telemetry_hott_unittest.cc | 2 +- 11 files changed, 38 insertions(+), 36 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 06066752e0..2e63e0c236 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -914,7 +914,7 @@ void activateConfig(void) #endif useFailsafeConfig(&masterConfig.failsafeConfig); - setAccelerationTrims(&accelerometerConfig()->accZero); + setAccelerationTrims(&accelerometerConfigMutable()->accZero); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); mixerUseConfigs( @@ -1083,13 +1083,13 @@ void validateAndFixGyroConfig(void) samplingTime = 0.00003125; // F1 and F3 can't handle high sample speed. #if defined(STM32F1) - gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16); #elif defined(STM32F3) - gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); #endif } else { #if defined(STM32F1) - gyroConfig()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); + gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4); #endif } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index dbf8815a72..65ba91f0d8 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1511,9 +1511,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #else motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif - motorConfig()->motorPwmRate = sbufReadU16(src); + motorConfigMutable()->motorPwmRate = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { - motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; + motorConfigMutable()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } if (sbufBytesRemaining(src)) { gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src); @@ -1525,7 +1525,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) validateAndFixGyroConfig(); if (sbufBytesRemaining(src)) { - motorConfig()->motorPwmInversion = sbufReadU8(src); + motorConfigMutable()->motorPwmInversion = sbufReadU8(src); } break; @@ -1812,7 +1812,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); } if (dataSize > 22) { - rxConfig()->fpvCamAngleDegrees = sbufReadU8(src); + rxConfigMutable()->fpvCamAngleDegrees = sbufReadU8(src); } break; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 719a9f9ff5..3b4d91ffbe 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -59,6 +59,8 @@ typedef struct throttleCorrectionConfig_s { uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. } throttleCorrectionConfig_t; +PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig); + typedef struct imuConfig_s { uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d79d135a54..faebb49cc9 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -45,7 +45,8 @@ #endif #include "io/serial.h" -#include "fc/cli.h" // for cliEnter() + +#include "fc/cli.h" #include "msp/msp_serial.h" @@ -53,7 +54,7 @@ #include "telemetry/telemetry.h" #endif -static serialConfig_t *serialConfig; +static const serialConfig_t *serialConfig; static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT]; const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { @@ -160,7 +161,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function) { while (findSerialPortConfigState.lastIndex < SERIAL_PORT_COUNT) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[findSerialPortConfigState.lastIndex++]; + serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[findSerialPortConfigState.lastIndex++]; if (candidate->functionMask & function) { return candidate; @@ -173,7 +174,7 @@ typedef struct findSharedSerialPortState_s { uint8_t lastIndex; } findSharedSerialPortState_t; -portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function) +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) { if (!portConfig || (portConfig->functionMask & function) == 0) { return PORTSHARING_UNUSED; @@ -181,7 +182,7 @@ portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFun return portConfig->functionMask == function ? PORTSHARING_NOT_SHARED : PORTSHARING_SHARED; } -bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction) { return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask); } @@ -198,10 +199,10 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) { while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[findSharedSerialPortState.lastIndex++]; + const serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++]; if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) { - serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); + const serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); if (!serialPortUsage) { continue; } @@ -218,7 +219,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX) #endif -bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) +bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) { UNUSED(serialConfigToCheck); /* @@ -232,9 +233,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) */ uint8_t mspPortCount = 0; - uint8_t index; - for (index = 0; index < SERIAL_PORT_COUNT; index++) { - serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index]; + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + const serialPortConfig_t *portConfig = &serialConfigToCheck->portConfigs[index]; if (portConfig->functionMask & FUNCTION_MSP) { mspPortCount++; @@ -268,9 +268,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier) { - uint8_t index; - for (index = 0; index < SERIAL_PORT_COUNT; index++) { - serialPortConfig_t *candidate = &serialConfig->portConfigs[index]; + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { + serialPortConfig_t *candidate = &serialConfigMutable()->portConfigs[index]; if (candidate->identifier == identifier) { return candidate; } @@ -397,7 +396,7 @@ void closeSerialPort(serialPort_t *serialPort) serialPortUsage->serialPort = NULL; } -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) +void serialInit(const serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) { uint8_t index; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index cfed31388c..482dad18fd 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + #include "config/parameter_group.h" #include "drivers/serial.h" @@ -119,18 +122,18 @@ typedef void serialConsumer(uint8_t); // // configuration // -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); +void serialInit(const serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); -bool isSerialConfigValid(serialConfig_t *serialConfig); +bool isSerialConfigValid(const serialConfig_t *serialConfig); serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function); -portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function); -bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function); +bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction); serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier); int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index c3f6993f76..96735549be 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -69,7 +69,7 @@ int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start #ifndef USE_PARAMETER_GROUPS -batteryConfig_t *batteryConfig; +const batteryConfig_t *batteryConfig; #endif static batteryState_e vBatState; @@ -213,7 +213,7 @@ const char * getBatteryStateString(void) return batteryStateStrings[getBatteryState()]; } -void batteryInit(batteryConfig_t *initialBatteryConfig) +void batteryInit(const batteryConfig_t *initialBatteryConfig) { #ifndef USE_PARAMETER_GROUPS (void)initialBatteryConfig; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 69be1bf98d..8e15303907 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -81,13 +81,13 @@ extern int32_t amperageLatest; extern int32_t amperage; extern int32_t mAhDrawn; #ifndef USE_PARAMETER_GROUPS -extern batteryConfig_t *batteryConfig; +extern const batteryConfig_t *batteryConfig; #endif batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); -void batteryInit(batteryConfig_t *initialBatteryConfig); +void batteryInit(const batteryConfig_t *initialBatteryConfig); struct rxConfig_s; void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc index a6b038441a..c422e24731 100644 --- a/src/test/unit/cms_unittest.cc +++ b/src/test/unit/cms_unittest.cc @@ -26,7 +26,7 @@ #define BARO extern "C" { - #include "platform.h"" + #include "platform.h" #include "target.h" #include "drivers/display.h" #include "cms/cms.h" diff --git a/src/test/unit/target.h b/src/test/unit/target.h index cc8149c790..741529284a 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -56,8 +56,6 @@ #define SERIAL_PORT_COUNT 8 -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 - #define TARGET_BOARD_IDENTIFIER "TEST" #define LED_STRIP_TIMER 1 diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index a5c4b8faa4..4fb0cea52f 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -306,7 +306,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} bool telemetryDetermineEnabledState(portSharing_e) {return true;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} +portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} uint8_t batteryCapacityRemainingPercentage(void) {return 67;} uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;} diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 54caa75e99..29d9cd59cf 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -252,7 +252,7 @@ bool telemetryDetermineEnabledState(portSharing_e) return true; } -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) +portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) { return PORTSHARING_NOT_SHARED; } From a3951a3340e8a1360d36dda2dd59da1026e54a9c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 1 Feb 2017 10:32:27 +0000 Subject: [PATCH 85/97] Preparation for conversion to parameter groups 3 --- src/main/config/parameter_group.h | 24 +++++++++++++++++++ src/main/fc/fc_init.c | 7 +----- src/main/sensors/gyro.c | 36 ++++++++++++++++++++--------- src/main/sensors/gyro.h | 2 +- src/main/sensors/initialisation.c | 38 ++++++++++++------------------- src/main/sensors/initialisation.h | 6 +---- 6 files changed, 67 insertions(+), 46 deletions(-) diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 8ee75f9968..33d49c6f4f 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -140,6 +140,7 @@ extern const uint8_t __pg_resetdata_end[]; #endif // USE_PARAMETER_GROUPS +#ifdef USE_PARAMETER_GROUPS // Register system config #define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ _type _name ## _System; \ @@ -242,6 +243,29 @@ extern const uint8_t __pg_resetdata_end[]; __VA_ARGS__ \ } \ /**/ +#else +#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ + _type _name ## _System + +#define PG_REGISTER(_type, _name, _pgn, _version) \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ + /**/ + +#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \ + extern void pgResetFn_ ## _name(_type *); \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \ + /**/ + +#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ + extern const _type pgResetTemplate_ ## _name; \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + /**/ +#define PG_RESET_TEMPLATE(_type, _name, ...) \ + const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \ + __VA_ARGS__ \ + } \ + /**/ +#endif const pgRegistry_t* pgFind(pgn_t pgn); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a5224e8508..4fce84f0de 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -403,12 +403,7 @@ void init(void) } #endif -#ifdef SONAR - const sonarConfig_t *sonarConfig = sonarConfig(); -#else - const void *sonarConfig = NULL; -#endif - if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) { + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 1fb1f66181..677bce6a84 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -72,9 +72,6 @@ gyro_t gyro; // gyro access functions static int32_t gyroADC[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; -#ifndef USE_PARAMETER_GROUPS -static const gyroConfig_t *gyroConfig; -#endif static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -86,6 +83,29 @@ static void *notchFilter2[3]; #define DEBUG_GYRO_CALIBRATION 3 +#ifdef STM32F10X +#define GYRO_SYNC_DENOM_DEFAULT 8 +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) +#define GYRO_SYNC_DENOM_DEFAULT 1 +#else +#define GYRO_SYNC_DENOM_DEFAULT 4 +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); + +PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, + .gyro_lpf = GYRO_LPF_256HZ, + .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, + .gyro_soft_lpf_type = FILTER_PT1, + .gyro_soft_lpf_hz = 90, + .gyro_soft_notch_hz_1 = 400, + .gyro_soft_notch_cutoff_1 = 300, + .gyro_soft_notch_hz_2 = 200, + .gyro_soft_notch_cutoff_2 = 100, + .gyro_align = ALIGN_DEFAULT, + .gyroMovementCalibrationThreshold = 32 +); + static const extiConfig_t *selectMPUIntExtiConfig(void) { #if defined(MPU_INT_EXTI) @@ -236,13 +256,8 @@ static bool gyroDetect(gyroDev_t *dev) return true; } -bool gyroInit(const gyroConfig_t *gyroConfigToUse) +bool gyroInit(void) { -#ifdef USE_PARAMETER_GROUPS - (void)(gyroConfigToUse); -#else - gyroConfig = gyroConfigToUse; -#endif memset(&gyro, 0, sizeof(gyro)); #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); @@ -257,8 +272,7 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse) switch (detectedSensors[SENSOR_INDEX_GYRO]) { default: // gyro does not support 32kHz - // cast away constness, legitimate as this is cross-validation - ((gyroConfig_t*)gyroConfig)->gyro_use_32khz = false; + gyroConfigMutable()->gyro_use_32khz = false; break; case GYRO_MPU6500: case GYRO_MPU9250: diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5bd68fcb91..94e86269bb 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -63,7 +63,7 @@ typedef struct gyroConfig_s { PG_DECLARE(gyroConfig_t, gyroConfig); void gyroSetCalibrationCycles(void); -bool gyroInit(const gyroConfig_t *gyroConfigToUse); +bool gyroInit(void); void gyroInitFilters(void); void gyroUpdate(void); bool isGyroCalibrationComplete(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 01b396b2c1..38d1dea7d4 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/utils.h" #include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -52,50 +54,40 @@ static bool sonarDetect(void) } #endif -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accelerometerConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *barometerConfig, - const sonarConfig_t *sonarConfig) +bool sensorsAutodetect(void) { // gyro must be initialised before accelerometer - if (!gyroInit(gyroConfig)) { + if (!gyroInit()) { return false; } - accInit(accelerometerConfig, gyro.targetLooptime); + accInit(accelerometerConfig(), gyro.targetLooptime); mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG - if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { - compassInit(compassConfig); + if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) { + compassInit(compassConfig()); } -#else - UNUSED(compassConfig); #endif #ifdef BARO - baroDetect(&baro.dev, barometerConfig->baro_hardware); -#else - UNUSED(barometerConfig); + baroDetect(&baro.dev, barometerConfig()->baro_hardware); #endif #ifdef SONAR if (sonarDetect()) { - sonarInit(sonarConfig); + sonarInit(sonarConfig()); } -#else - UNUSED(sonarConfig); #endif - if (gyroConfig->gyro_align != ALIGN_DEFAULT) { - gyro.dev.gyroAlign = gyroConfig->gyro_align; + if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { + gyro.dev.gyroAlign = gyroConfig()->gyro_align; } - if (accelerometerConfig->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accelerometerConfig->acc_align; + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc_align; } - if (compassConfig->mag_align != ALIGN_DEFAULT) { - mag.dev.magAlign = compassConfig->mag_align; + if (compassConfig()->mag_align != ALIGN_DEFAULT) { + mag.dev.magAlign = compassConfig()->mag_align; } return true; diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index e656294c6f..9b5d045046 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,8 +17,4 @@ #pragma once -bool sensorsAutodetect(const gyroConfig_t *gyroConfig, - const accelerometerConfig_t *accConfig, - const compassConfig_t *compassConfig, - const barometerConfig_t *baroConfig, - const sonarConfig_t *sonarConfig); +bool sensorsAutodetect(void); From 5851b21e4ad03b18c028709bc6595498d745b7bc Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 1 Feb 2017 12:58:29 +0000 Subject: [PATCH 86/97] Removed a number of static config pointers --- src/main/fc/config.c | 10 +----- src/main/fc/fc_init.c | 10 +++--- src/main/fc/fc_tasks.c | 2 +- src/main/flight/failsafe.c | 53 ++++++++++--------------------- src/main/flight/failsafe.h | 5 ++- src/main/io/serial.c | 14 +++----- src/main/io/serial.h | 2 +- src/main/rx/jetiexbus.c | 5 +-- src/main/sensors/acceleration.c | 4 +-- src/main/sensors/acceleration.h | 2 +- src/main/sensors/barometer.c | 13 -------- src/main/sensors/barometer.h | 1 - src/main/sensors/compass.c | 6 ++-- src/main/sensors/compass.h | 2 +- src/main/sensors/initialisation.c | 4 +-- src/main/telemetry/frsky.c | 14 ++------ src/main/telemetry/frsky.h | 6 ++-- src/main/telemetry/hott.c | 10 +----- src/main/telemetry/hott.h | 2 +- src/main/telemetry/jetiexbus.h | 3 +- src/main/telemetry/ltm.c | 10 +----- src/main/telemetry/ltm.h | 3 +- src/main/telemetry/smartport.c | 10 +----- src/main/telemetry/smartport.h | 2 +- src/main/telemetry/telemetry.c | 29 ++++------------- src/main/telemetry/telemetry.h | 18 +++++------ 26 files changed, 67 insertions(+), 173 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2e63e0c236..232399a9a7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -904,16 +904,12 @@ void activateConfig(void) ¤tProfile->pidProfile ); -#ifdef TELEMETRY - telemetryUseConfig(&masterConfig.telemetryConfig); -#endif - #ifdef GPS gpsUseProfile(&masterConfig.gpsProfile); gpsUsePIDs(¤tProfile->pidProfile); #endif - useFailsafeConfig(&masterConfig.failsafeConfig); + failsafeReset(); setAccelerationTrims(&accelerometerConfigMutable()->accZero); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); @@ -942,10 +938,6 @@ void activateConfig(void) &masterConfig.rcControlsConfig, &masterConfig.motorConfig ); - -#ifdef BARO - useBarometerConfig(&masterConfig.barometerConfig); -#endif } void validateAndFixConfig(void) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 4fce84f0de..4e24d8772c 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -259,16 +259,16 @@ void init(void) timerInit(); // timer must be initialized before any channel is allocated #if defined(AVOID_UART1_FOR_PWM_PPM) - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), + serialInit(feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), + serialInit(feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), + serialInit(feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else - serialInit(serialConfig(), feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); + serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif mixerInit(mixerConfig()->mixerMode, masterConfig.customMotorMixer); @@ -443,7 +443,7 @@ void init(void) cliInit(serialConfig()); #endif - failsafeInit(rxConfig(), flight3DConfig()->deadband3d_throttle); + failsafeInit(flight3DConfig()->deadband3d_throttle); rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a729abe88a..d37278147e 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -204,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs) telemetryCheckState(); if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(currentTimeUs, rxConfig(), flight3DConfig()->deadband3d_throttle); + telemetryProcess(currentTimeUs); } } #endif diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index f0d2a9e16f..2ef72459b1 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -43,9 +43,9 @@ /* * Usage: * - * failsafeInit() and useFailsafeConfig() must be called before the other methods are used. + * failsafeInit() and resetFailsafe() must be called before the other methods are used. * - * failsafeInit() and useFailsafeConfig() can be called in any order. + * failsafeInit() and resetFailsafe() can be called in any order. * failsafeInit() should only be called once. * * enable() should be called after system initialisation. @@ -53,16 +53,14 @@ static failsafeState_t failsafeState; -#ifndef USE_PARAMETER_GROPUS -static const failsafeConfig_t *failsafeConfig; -static const rxConfig_t *rxConfig; -#endif - static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC -static void failsafeReset(void) +/* + * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. + */ +void failsafeReset(void) { - failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig->failsafe_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; failsafeState.validRxDataReceivedAt = 0; failsafeState.validRxDataFailedAt = 0; failsafeState.throttleLowPeriod = 0; @@ -73,27 +71,8 @@ static void failsafeReset(void) failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } -/* - * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. - */ -void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse) +void failsafeInit(uint16_t deadband3d_throttle) { -#ifdef USE_PARAMETER_GROUPS - (void)(failsafeConfigToUse); -#else - failsafeConfig = failsafeConfigToUse; -#endif - failsafeReset(); -} - -void failsafeInit(const rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle) -{ -#ifdef USE_PARAMETER_GROUPS - (void)(intialRxConfig); -#else - rxConfig = intialRxConfig; -#endif - deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; failsafeState.monitoring = false; @@ -131,7 +110,7 @@ static void failsafeActivate(void) failsafeState.active = true; failsafeState.phase = FAILSAFE_LANDING; ENABLE_FLIGHT_MODE(FAILSAFE_MODE); - failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; failsafeState.events++; } @@ -139,9 +118,9 @@ static void failsafeActivate(void) static void failsafeApplyControlInput(void) { for (int i = 0; i < 3; i++) { - rcData[i] = rxConfig->midrc; + rcData[i] = rxConfig()->midrc; } - rcData[THROTTLE] = failsafeConfig->failsafe_throttle; + rcData[THROTTLE] = failsafeConfig()->failsafe_throttle; } bool failsafeIsReceivingRxData(void) @@ -201,11 +180,11 @@ void failsafeUpdateState(void) case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time - if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) { - failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; + if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig(), deadband3dThrottle)) { + failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) - if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) { + if (failsafeSwitchIsOn && failsafeConfig()->failsafe_kill_switch) { // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure @@ -238,7 +217,7 @@ void failsafeUpdateState(void) if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { - switch (failsafeConfig->failsafe_procedure) { + switch (failsafeConfig()->failsafe_procedure) { default: case FAILSAFE_PROCEDURE_AUTO_LANDING: // Stabilize, and set Throttle to specified level @@ -300,7 +279,7 @@ void failsafeUpdateState(void) // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. // This is to prevent that JustDisarm is activated on the next iteration. // Because that would have the effect of shutting down failsafe handling on intermittent connections. - failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; DISABLE_FLIGHT_MODE(FAILSAFE_MODE); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index be32b769e5..2feefb297d 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -74,9 +74,8 @@ typedef struct failsafeState_s { failsafeRxLinkState_e rxLinkState; } failsafeState_t; -struct rxConfig_s; -void failsafeInit(const struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); -void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse); +void failsafeInit(uint16_t deadband3d_throttle); +void failsafeReset(void); void failsafeStartMonitoring(void); void failsafeUpdateState(void); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index faebb49cc9..cb12d7ff82 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -54,7 +54,6 @@ #include "telemetry/telemetry.h" #endif -static const serialConfig_t *serialConfig; static serialPortUsage_t serialPortUsageList[SERIAL_PORT_COUNT]; const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { @@ -396,16 +395,12 @@ void closeSerialPort(serialPort_t *serialPort) serialPortUsage->serialPort = NULL; } -void serialInit(const serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) +void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) { - uint8_t index; - - serialConfig = initialSerialConfig; - serialPortCount = SERIAL_PORT_COUNT; memset(&serialPortUsageList, 0, sizeof(serialPortUsageList)); - for (index = 0; index < SERIAL_PORT_COUNT; index++) { + for (int index = 0; index < SERIAL_PORT_COUNT; index++) { serialPortUsageList[index].identifier = serialPortIdentifiers[index]; if (serialPortToDisable != SERIAL_PORT_NONE) { @@ -471,7 +466,7 @@ void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar) cliEnter(serialPort); } #endif - if (receivedChar == serialConfig->reboot_character) { + if (receivedChar == serialConfig()->reboot_character) { systemResetToBootloader(); } } @@ -488,8 +483,7 @@ static void nopConsumer(uint8_t data) arbitrary serial passthrough "proxy". Optional callbacks can be given to allow for specialized data processing. */ -void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer - *leftC, serialConsumer *rightC) +void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC) { waitForSerialPortToFinishTransmitting(left); waitForSerialPortToFinishTransmitting(right); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 482dad18fd..0398665a8d 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -122,7 +122,7 @@ typedef void serialConsumer(uint8_t); // // configuration // -void serialInit(const serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); +void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index ba85e9bd55..72b236e1a4 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -407,10 +407,8 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uin ----------------------------------------------- */ -void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig) +void initJetiExBusTelemetry(void) { - UNUSED(initialTelemetryConfig); - // Init Ex Bus Frame header jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B; // Startbytes jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01; @@ -427,7 +425,6 @@ void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig) jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 } - void createExTelemetrieTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor) { uint8_t labelLength = strlen(sensor->label); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d8395f141e..0d9289ede8 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -246,13 +246,13 @@ retry: return true; } -bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval) +bool accInit(uint32_t gyroSamplingInverval) { memset(&acc, 0, sizeof(acc)); // copy over the common gyro mpu settings acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration; acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult; - if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) { + if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { return false; } acc.dev.acc_1G = 256; // set default diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 70905bb3a9..35de76374e 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -69,7 +69,7 @@ typedef struct accelerometerConfig_s { PG_DECLARE(accelerometerConfig_t, accelerometerConfig); -bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime); +bool accInit(uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 42a013259b..4b25477dc5 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -54,10 +54,6 @@ static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; static uint32_t baroPressureSum = 0; -#ifndef USE_PARAMETER_GROUPS -static const barometerConfig_t *barometerConfig; -#endif - bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function @@ -124,15 +120,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) return true; } -void useBarometerConfig(const barometerConfig_t *barometerConfigToUse) -{ -#ifdef USE_PARAMETER_GROUPS - (void)(barometerConfigToUse); -#else - barometerConfig = barometerConfigToUse; -#endif -} - bool isBaroCalibrationComplete(void) { return calibratingB == 0; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 1f6e6687fa..b753ef5eaa 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -49,7 +49,6 @@ typedef struct baro_s { extern baro_t baro; bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse); -void useBarometerConfig(const barometerConfig_t *barometerConfigToUse); bool isBaroCalibrationComplete(void); void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired); uint32_t baroUpdate(void); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index be53d8c9b6..8e84e44b46 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -146,12 +146,12 @@ retry: return true; } -void compassInit(const compassConfig_t *compassConfig) +void compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // calculate magnetic declination - const int16_t deg = compassConfig->mag_declination / 100; - const int16_t min = compassConfig->mag_declination % 100; + const int16_t deg = compassConfig()->mag_declination / 100; + const int16_t min = compassConfig()->mag_declination % 100; mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; mag.dev.init(); diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 59b997d76f..4f95e9db55 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -50,7 +50,7 @@ typedef struct compassConfig_s { PG_DECLARE(compassConfig_t, compassConfig); bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); -void compassInit(const compassConfig_t *compassConfig); +void compassInit(void); union flightDynamicsTrims_u; void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 38d1dea7d4..4220fab3be 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -61,12 +61,12 @@ bool sensorsAutodetect(void) return false; } - accInit(accelerometerConfig(), gyro.targetLooptime); + accInit(gyro.targetLooptime); mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. #ifdef MAG if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) { - compassInit(compassConfig()); + compassInit(); } #endif diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 3dad42c3fc..b764353caa 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -74,9 +74,6 @@ static serialPortConfig_t *portConfig; #define FRSKY_BAUDRATE 9600 #define FRSKY_INITIAL_PORT_MODE MODE_TX -#ifndef USE_PARAMETER_GROUPS -static const telemetryConfig_t *telemetryConfig; -#endif static bool frskyTelemetryEnabled = false; static portSharing_e frskyPortSharing; @@ -457,13 +454,8 @@ static void sendHeading(void) serialize16(0); } -void initFrSkyTelemetry(const telemetryConfig_t *initialTelemetryConfig) +void initFrSkyTelemetry(void) { -#ifdef USE_PARAMETER_GROUPS - UNUSED(initialTelemetryConfig); -#else - telemetryConfig = initialTelemetryConfig; -#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); } @@ -515,7 +507,7 @@ void checkFrSkyTelemetryState(void) } } -void handleFrSkyTelemetry(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void handleFrSkyTelemetry(void) { if (!frskyTelemetryEnabled) { return; @@ -546,7 +538,7 @@ void handleFrSkyTelemetry(const rxConfig_t *rxConfig, uint16_t deadband3d_thrott if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle); + sendThrottleOrBatterySizeAsRpm(rxConfig(), flight3DConfig()->deadband3d_throttle); if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) { sendVoltage(); diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 419b4b407b..255dae90de 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -22,12 +22,10 @@ typedef enum { FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -struct rxConfig_s; -void handleFrSkyTelemetry(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void handleFrSkyTelemetry(void); void checkFrSkyTelemetryState(void); -struct telemetryConfig_s; -void initFrSkyTelemetry(const struct telemetryConfig_s *telemetryConfig); +void initFrSkyTelemetry(void); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index dacf2393f1..dfae08c7e8 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -112,9 +112,6 @@ static uint8_t hottMsgCrc; static serialPort_t *hottPort = NULL; static serialPortConfig_t *portConfig; -#ifndef USE_PARAMETER_GROUPS -static const telemetryConfig_t *telemetryConfig; -#endif static bool hottTelemetryEnabled = false; static portSharing_e hottPortSharing; @@ -295,13 +292,8 @@ void freeHoTTTelemetryPort(void) hottTelemetryEnabled = false; } -void initHoTTTelemetry(const telemetryConfig_t *initialTelemetryConfig) +void initHoTTTelemetry(void) { -#ifdef USE_PARAMETER_GROUPS - UNUSED(initialTelemetryConfig); -#else - telemetryConfig = initialTelemetryConfig; -#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index bd437afa93..660c4266ab 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -490,7 +490,7 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(timeUs_t currentTimeUs); void checkHoTTTelemetryState(void); -void initHoTTTelemetry(const telemetryConfig_t *telemetryConfig); +void initHoTTTelemetry(void); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/jetiexbus.h b/src/main/telemetry/jetiexbus.h index 79440c0df2..9c8327db4a 100644 --- a/src/main/telemetry/jetiexbus.h +++ b/src/main/telemetry/jetiexbus.h @@ -17,7 +17,6 @@ #pragma once -struct telemetryConfig_s; -void initJetiExBusTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initJetiExBusTelemetry(void); void checkJetiExBusTelemetryState(void); void handleJetiExBusTelemetry(void); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index ab5a237db1..fec48d7b75 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -81,9 +81,6 @@ static serialPort_t *ltmPort; static serialPortConfig_t *portConfig; -#ifndef USE_PARAMETER_GROUPS -static const telemetryConfig_t *telemetryConfig; -#endif static bool ltmEnabled; static portSharing_e ltmPortSharing; static uint8_t ltm_crc; @@ -271,13 +268,8 @@ void freeLtmTelemetryPort(void) ltmEnabled = false; } -void initLtmTelemetry(const telemetryConfig_t *initialTelemetryConfig) +void initLtmTelemetry(void) { -#ifdef USE_PARAMETER_GROUPS - UNUSED(initialTelemetryConfig); -#else - telemetryConfig = initialTelemetryConfig; -#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM); ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM); } diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index e0603707da..892d0efdd8 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -19,8 +19,7 @@ #pragma once -struct telemetryConfig_s; -void initLtmTelemetry(const struct telemetryConfig_s *initialTelemetryConfig); +void initLtmTelemetry(void); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 13e09f4c6c..7698b4e385 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -150,9 +150,6 @@ const uint16_t frSkyDataIdTable[] = { static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port. static serialPortConfig_t *portConfig; -#ifndef USE_PARAMETER_GROUPS -static const telemetryConfig_t *telemetryConfig; -#endif static bool smartPortTelemetryEnabled = false; static portSharing_e smartPortPortSharing; @@ -306,13 +303,8 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) smartPortSendPackageEx(FSSP_DATA_FRAME,payload); } -void initSmartPortTelemetry(const telemetryConfig_t *initialTelemetryConfig) +void initSmartPortTelemetry(void) { -#ifdef USE_PARAMETER_GROUPS - UNUSED(initialTelemetryConfig); -#else - telemetryConfig = initialTelemetryConfig; -#endif portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT); smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT); } diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index e7d7722b2c..35f18fc4ae 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,7 +7,7 @@ #pragma once -void initSmartPortTelemetry(const telemetryConfig_t *); +void initSmartPortTelemetry(void); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 0d635d793b..f1cda235a9 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -53,35 +53,23 @@ #include "telemetry/srxl.h" #include "telemetry/ibus.h" -#ifndef USE_PARAMETER_GROUPS -static telemetryConfig_t *telemetryConfig; -#endif - -void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse) -{ -#ifdef USE_PARAMETER_GROUPS - UNUSED(telemetryConfigToUse); -#else - telemetryConfig = telemetryConfigToUse; -#endif -} void telemetryInit(void) { #ifdef TELEMETRY_FRSKY - initFrSkyTelemetry(telemetryConfig()); + initFrSkyTelemetry(); #endif #ifdef TELEMETRY_HOTT - initHoTTTelemetry(telemetryConfig()); + initHoTTTelemetry(); #endif #ifdef TELEMETRY_SMARTPORT - initSmartPortTelemetry(telemetryConfig()); + initSmartPortTelemetry(); #endif #ifdef TELEMETRY_LTM - initLtmTelemetry(telemetryConfig()); + initLtmTelemetry(); #endif #ifdef TELEMETRY_JETIEXBUS - initJetiExBusTelemetry(telemetryConfig()); + initJetiExBusTelemetry(); #endif #ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); @@ -151,13 +139,10 @@ void telemetryCheckState(void) #endif } -void telemetryProcess(uint32_t currentTime, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void telemetryProcess(uint32_t currentTime) { #ifdef TELEMETRY_FRSKY - handleFrSkyTelemetry(rxConfig, deadband3d_throttle); -#else - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); + handleFrSkyTelemetry(); #endif #ifdef TELEMETRY_HOTT handleHoTTTelemetry(currentTime); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 34f59f2711..0fc00cae57 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -38,11 +38,11 @@ typedef enum { } frskyUnit_e; typedef struct telemetryConfig_s { + float gpsNoFixLatitude; + float gpsNoFixLongitude; uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inversion; // also shared with smartport inversion uint8_t sportHalfDuplex; - float gpsNoFixLatitude; - float gpsNoFixLongitude; frskyGpsCoordFormat_e frsky_coordinate_format; frskyUnit_e frsky_unit; uint8_t frsky_vfas_precision; @@ -54,18 +54,16 @@ typedef struct telemetryConfig_s { PG_DECLARE(telemetryConfig_t, telemetryConfig); -void telemetryInit(void); -bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); +#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) + extern serialPort_t *telemetrySharedPort; +void telemetryInit(void); +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); + void telemetryCheckState(void); -struct rxConfig_s; -void telemetryProcess(uint32_t currentTime, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void telemetryProcess(uint32_t currentTime); bool telemetryDetermineEnabledState(portSharing_e portSharing); -void telemetryUseConfig(telemetryConfig_t *telemetryConfig); - -#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) - void releaseSharedTelemetryPorts(void); From 07dc35d441a3123bdc79ea5fc210ad726e7e2539 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 1 Feb 2017 13:09:46 +0000 Subject: [PATCH 87/97] Removed mixer and battery config static copies --- src/main/fc/config.c | 8 +------- src/main/fc/fc_init.c | 2 +- src/main/flight/mixer.c | 26 ++------------------------ src/main/flight/mixer.h | 7 +------ src/main/sensors/battery.c | 11 +---------- src/main/sensors/battery.h | 2 +- 6 files changed, 7 insertions(+), 49 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 232399a9a7..0ab826f33e 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -913,13 +913,7 @@ void activateConfig(void) setAccelerationTrims(&accelerometerConfigMutable()->accZero); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); - mixerUseConfigs( - &masterConfig.flight3DConfig, - &masterConfig.motorConfig, - &masterConfig.mixerConfig, - &masterConfig.airplaneConfig, - &masterConfig.rxConfig - ); + mixerUseConfigs(&masterConfig.airplaneConfig); #ifdef USE_SERVOS servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.gimbalConfig); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 4e24d8772c..f1652946b6 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -551,7 +551,7 @@ void init(void) // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) - batteryInit(batteryConfig()); + batteryInit(); #ifdef USE_DASHBOARD if (feature(FEATURE_DASHBOARD)) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ddac5029db..fd36ad76ea 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -63,12 +63,6 @@ int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; static airplaneConfig_t *airplaneConfig; -#ifndef USE_PARAMETER_GROUPS -static const mixerConfig_t *mixerConfig; -static const flight3DConfig_t *flight3DConfig; -static const motorConfig_t *motorConfig; -const rxConfig_t *rxConfig; -#endif mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -297,24 +291,8 @@ void initEscEndpoints(void) { rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle; } -void mixerUseConfigs( - flight3DConfig_t *flight3DConfigToUse, - motorConfig_t *motorConfigToUse, - mixerConfig_t *mixerConfigToUse, - airplaneConfig_t *airplaneConfigToUse, - rxConfig_t *rxConfigToUse) +void mixerUseConfigs(airplaneConfig_t *airplaneConfigToUse) { -#ifdef USE_PARAMETER_GROUPS - (void)(flight3DConfigToUse); - (void)(motorConfigToUse); - (void)(mixerConfigToUse); - (void)(rxConfigToUse); -#else - flight3DConfig = flight3DConfigToUse; - motorConfig = motorConfigToUse; - mixerConfig = mixerConfigToUse; - rxConfig = rxConfigToUse; -#endif airplaneConfig = airplaneConfigToUse; } @@ -494,7 +472,7 @@ void mixTable(pidProfile_t *pidProfile) } // Calculate voltage compensation - const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; + const float vbatCompensationFactor = pidProfile->vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f; // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 6a206f4e86..66baf8172b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -141,12 +141,7 @@ struct rxConfig_s; uint8_t getMotorCount(); float getMotorMixRange(); -void mixerUseConfigs( - flight3DConfig_t *flight3DConfigToUse, - motorConfig_t *motorConfigToUse, - mixerConfig_t *mixerConfigToUse, - airplaneConfig_t *airplaneConfigToUse, - struct rxConfig_s *rxConfigToUse); +void mixerUseConfigs(airplaneConfig_t *airplaneConfigToUse); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 96735549be..02f53ffa93 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -68,10 +68,6 @@ int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start -#ifndef USE_PARAMETER_GROUPS -const batteryConfig_t *batteryConfig; -#endif - static batteryState_e vBatState; static batteryState_e consumptionState; @@ -213,13 +209,8 @@ const char * getBatteryStateString(void) return batteryStateStrings[getBatteryState()]; } -void batteryInit(const batteryConfig_t *initialBatteryConfig) +void batteryInit(void) { -#ifndef USE_PARAMETER_GROUPS - (void)initialBatteryConfig; -#else - batteryConfig = initialBatteryConfig; -#endif vBatState = BATTERY_NOT_PRESENT; consumptionState = BATTERY_OK; batteryCellCount = 0; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 8e15303907..a8e21145ff 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -87,7 +87,7 @@ extern const batteryConfig_t *batteryConfig; batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); -void batteryInit(const batteryConfig_t *initialBatteryConfig); +void batteryInit(void); struct rxConfig_s; void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); From ef549036040d8774a9ec9417f49e08131e9e5fc2 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sun, 29 Jan 2017 18:07:18 +0100 Subject: [PATCH 88/97] F7 SPI HAL driver fix --- src/main/drivers/bus_spi_hal.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index e27f8fa652..244cb5e9e5 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -72,7 +72,7 @@ static spiDevice_t spiHardwareMap[] = { { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER }, { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF5_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER }, + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER }, { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER } }; From 22d11e35b295087c696fa836141329f234d51e77 Mon Sep 17 00:00:00 2001 From: Bryce Johnson Date: Sun, 29 Jan 2017 21:32:55 -0600 Subject: [PATCH 89/97] Added blackbox erase for flashchips revert back initBlackbox Fixed blackbox erase when full. Change BOXBLACKBOXERASE to the end Blackbox erase changes Moved the ARM detect into the blackbox statemachine Added blackbox erase start and end beep Tab to space --- src/main/blackbox/blackbox.c | 144 ++++++++++++++++++------------- src/main/blackbox/blackbox_io.c | 45 ++++++++++ src/main/blackbox/blackbox_io.h | 4 + src/main/cms/cms_menu_blackbox.c | 2 + src/main/fc/cli.c | 2 +- src/main/fc/fc_core.c | 9 -- src/main/fc/fc_msp.c | 3 + src/main/fc/rc_controls.h | 1 + src/main/io/beeper.c | 6 +- src/main/io/beeper.h | 2 +- 10 files changed, 143 insertions(+), 75 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 45b3bc1dbc..eb9aee930d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -241,21 +241,21 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { typedef enum BlackboxState { BLACKBOX_STATE_DISABLED = 0, - BLACKBOX_STATE_STOPPED, - BLACKBOX_STATE_PREPARE_LOG_FILE, - BLACKBOX_STATE_SEND_HEADER, - BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, - BLACKBOX_STATE_SEND_GPS_H_HEADER, - BLACKBOX_STATE_SEND_GPS_G_HEADER, - BLACKBOX_STATE_SEND_SLOW_HEADER, - BLACKBOX_STATE_SEND_SYSINFO, - BLACKBOX_STATE_PAUSED, - BLACKBOX_STATE_RUNNING, - BLACKBOX_STATE_SHUTTING_DOWN + BLACKBOX_STATE_STOPPED, //1 + BLACKBOX_STATE_PREPARE_LOG_FILE, //2 + BLACKBOX_STATE_SEND_HEADER, //3 + BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4 + BLACKBOX_STATE_SEND_GPS_H_HEADER, //5 + BLACKBOX_STATE_SEND_GPS_G_HEADER, //6 + BLACKBOX_STATE_SEND_SLOW_HEADER, //7 + BLACKBOX_STATE_SEND_SYSINFO, //8 + BLACKBOX_STATE_PAUSED, //9 + BLACKBOX_STATE_RUNNING, //10 + BLACKBOX_STATE_SHUTTING_DOWN, //11 + BLACKBOX_STATE_START_ERASE, //12 + BLACKBOX_STATE_ERASING, //13 } BlackboxState; -#define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER -#define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO typedef struct blackboxMainState_s { uint32_t time; @@ -804,47 +804,44 @@ void validateBlackboxConfig() */ void startBlackbox(void) { - if (blackboxState == BLACKBOX_STATE_STOPPED) { - validateBlackboxConfig(); + validateBlackboxConfig(); - if (!blackboxDeviceOpen()) { - blackboxSetState(BLACKBOX_STATE_DISABLED); - return; - } - - memset(&gpsHistory, 0, sizeof(gpsHistory)); - - blackboxHistory[0] = &blackboxHistoryRing[0]; - blackboxHistory[1] = &blackboxHistoryRing[1]; - blackboxHistory[2] = &blackboxHistoryRing[2]; - - vbatReference = vbatLatest; - - //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it - - /* - * We use conditional tests to decide whether or not certain fields should be logged. Since our headers - * must always agree with the logged data, the results of these tests must not change during logging. So - * cache those now. - */ - blackboxBuildConditionCache(); - - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); - - blackboxIteration = 0; - blackboxPFrameIndex = 0; - blackboxIFrameIndex = 0; - - /* - * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when - * it finally plays the beep for this arming event. - */ - blackboxLastArmingBeep = getArmingBeepTimeMicros(); - blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status - - - blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); + if (!blackboxDeviceOpen()) { + blackboxSetState(BLACKBOX_STATE_DISABLED); + return; } + + memset(&gpsHistory, 0, sizeof(gpsHistory)); + + blackboxHistory[0] = &blackboxHistoryRing[0]; + blackboxHistory[1] = &blackboxHistoryRing[1]; + blackboxHistory[2] = &blackboxHistoryRing[2]; + + vbatReference = vbatLatest; + + //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it + + /* + * We use conditional tests to decide whether or not certain fields should be logged. Since our headers + * must always agree with the logged data, the results of these tests must not change during logging. So + * cache those now. + */ + blackboxBuildConditionCache(); + + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); + + blackboxIteration = 0; + blackboxPFrameIndex = 0; + blackboxIFrameIndex = 0; + + /* + * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when + * it finally plays the beep for this arming event. + */ + blackboxLastArmingBeep = getArmingBeepTimeMicros(); + blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status + + blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } /** @@ -1469,18 +1466,26 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) void handleBlackbox(timeUs_t currentTimeUs) { int i; - - if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) { - blackboxReplenishHeaderBudget(); - } + static bool erasedOnce = false; //Only allow one erase per FC reboot. switch (blackboxState) { + case BLACKBOX_STATE_STOPPED: + if (ARMING_FLAG(ARMED)) { + blackboxOpen(); + startBlackbox(); + } + if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE) && !erasedOnce) { + blackboxSetState(BLACKBOX_STATE_START_ERASE); + erasedOnce = true; + } + break; case BLACKBOX_STATE_PREPARE_LOG_FILE: if (blackboxDeviceBeginLog()) { blackboxSetState(BLACKBOX_STATE_SEND_HEADER); } break; case BLACKBOX_STATE_SEND_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised /* @@ -1501,6 +1506,7 @@ void handleBlackbox(timeUs_t currentTimeUs) } break; case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { @@ -1514,6 +1520,7 @@ void handleBlackbox(timeUs_t currentTimeUs) break; #ifdef GPS case BLACKBOX_STATE_SEND_GPS_H_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAY_LENGTH(blackboxGpsHFields), NULL, NULL)) { @@ -1521,6 +1528,7 @@ void handleBlackbox(timeUs_t currentTimeUs) } break; case BLACKBOX_STATE_SEND_GPS_G_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAY_LENGTH(blackboxGpsGFields), &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) { @@ -1529,6 +1537,7 @@ void handleBlackbox(timeUs_t currentTimeUs) break; #endif case BLACKBOX_STATE_SEND_SLOW_HEADER: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAY_LENGTH(blackboxSlowFields), NULL, NULL)) { @@ -1536,6 +1545,7 @@ void handleBlackbox(timeUs_t currentTimeUs) } break; case BLACKBOX_STATE_SEND_SYSINFO: + blackboxReplenishHeaderBudget(); //On entry of this state, xmitState.headerIndex is 0 //Keep writing chunks of the system info headers until it returns true to signal completion @@ -1565,7 +1575,6 @@ void handleBlackbox(timeUs_t currentTimeUs) blackboxLogIteration(currentTimeUs); } - // Keep the logging timers ticking so our log iteration continues to advance blackboxAdvanceIterationTimers(); break; @@ -1577,7 +1586,6 @@ void handleBlackbox(timeUs_t currentTimeUs) } else { blackboxLogIteration(currentTimeUs); } - blackboxAdvanceIterationTimers(); break; case BLACKBOX_STATE_SHUTTING_DOWN: @@ -1594,15 +1602,29 @@ void handleBlackbox(timeUs_t currentTimeUs) blackboxSetState(BLACKBOX_STATE_STOPPED); } break; + case BLACKBOX_STATE_START_ERASE: + blackboxEraseAll(); + blackboxSetState(BLACKBOX_STATE_ERASING); + beeper(BEEPER_BLACKBOX_ERASE); + break; + case BLACKBOX_STATE_ERASING: + if (isBlackboxErased()) { + //Done eraseing + blackboxSetState(BLACKBOX_STATE_STOPPED); + beeper(BEEPER_BLACKBOX_ERASE); + } + default: break; } // Did we run out of room on the device? Stop! if (isBlackboxDeviceFull()) { - blackboxSetState(BLACKBOX_STATE_STOPPED); - // ensure we reset the test mode flag if we stop due to full memory card - if (startedLoggingInTestMode) startedLoggingInTestMode = false; + if (blackboxState != BLACKBOX_STATE_ERASING && blackboxState != BLACKBOX_STATE_START_ERASE) { + blackboxSetState(BLACKBOX_STATE_STOPPED); + // ensure we reset the test mode flag if we stop due to full memory card + if (startedLoggingInTestMode) startedLoggingInTestMode = false; + } } else { // Only log in test mode if there is room! if(blackboxConfig()->on_motor_test) { diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 356a98e5ee..f33e738160 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -66,6 +66,14 @@ static struct { #endif +void blackboxOpen() +{ + serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); + if (sharedBlackboxAndMspPort) { + mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); + } +} + void blackboxWrite(uint8_t value) { switch (blackboxConfig()->device) { @@ -602,6 +610,43 @@ bool blackboxDeviceOpen(void) } } +/** + * Erase all blackbox logs + */ +void blackboxEraseAll(void) +{ + switch (blackboxConfig()->device) { +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + flashfsEraseCompletely(); + break; +#endif + default: + //not supported + break; + + } +} + +/** + * Check to see if erasing is done + */ +bool isBlackboxErased(void) +{ + switch (blackboxConfig()->device) { +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + return flashfsIsReady(); + break; +#endif + default: + //not supported + return true; + break; + + } +} + /** * Close the Blackbox logging device immediately without attempting to flush any remaining data. */ diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 8a8087f180..9dc7b7566b 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -49,6 +49,7 @@ typedef enum { extern int32_t blackboxHeaderBudget; +void blackboxOpen(void); void blackboxWrite(uint8_t value); int blackboxPrintf(const char *fmt, ...); @@ -71,6 +72,9 @@ bool blackboxDeviceFlushForce(void); bool blackboxDeviceOpen(void); void blackboxDeviceClose(void); +void blackboxEraseAll(void); +bool isBlackboxErased(void); + bool blackboxDeviceBeginLog(void); bool blackboxDeviceEndLog(bool retainLog); diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index b109e64562..f8b2372316 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -46,6 +46,7 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" +#include "io/beeper.h" #include "blackbox/blackbox_io.h" @@ -63,6 +64,7 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr) delay(100); } + beeper(BEEPER_BLACKBOX_ERASE); displayClearScreen(pDisplay); displayResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI? diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d324488b4e..4039178cbf 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2729,7 +2729,7 @@ static void cliFlashErase(char *cmdline) #endif delay(100); } - + beeper(BEEPER_BLACKBOX_ERASE); cliPrintf("\r\nDone.\r\n"); } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e32e3e7b9c..57b926142c 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -188,15 +188,6 @@ void mwArm(void) ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); - if (sharedBlackboxAndMspPort) { - mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); - } - startBlackbox(); - } -#endif disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero //beep to indicate arming diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 65ba91f0d8..43175c83c5 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -150,6 +150,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXAIRMODE, "AIR MODE;", 28 }, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29}, { BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30}, + { BOXBLACKBOXERASE, "BLACKBOX ERASE;", 31 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -378,6 +379,7 @@ void initActiveBoxIds(void) #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOXERASE; } #endif @@ -442,6 +444,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 4f2533afd1..bf6ac276b6 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -53,6 +53,7 @@ typedef enum { BOXAIRMODE, BOX3DDISABLESWITCH, BOXFPVANGLEMIX, + BOXBLACKBOXERASE, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 13eefa691a..6f0e0ce6ee 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -171,9 +171,9 @@ typedef struct beeperTableEntry_s { { BEEPER_ENTRY(BEEPER_ARMED, 15, beep_armedBeep, "ARMED") }, { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 16, NULL, "SYSTEM_INIT") }, { BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") }, - - { BEEPER_ENTRY(BEEPER_ALL, 18, NULL, "ALL") }, - { BEEPER_ENTRY(BEEPER_PREFERENCE, 19, NULL, "PREFERRED") }, + { BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") }, + { BEEPER_ENTRY(BEEPER_ALL, 19, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_PREFERENCE, 20, NULL, "PREFERRED") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 15a40eef5d..645ff45d71 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -41,7 +41,7 @@ typedef enum { BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on BEEPER_USB, // Some boards have beeper powered USB connected - + BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes BEEPER_ALL, // Turn ON or OFF all beeper conditions BEEPER_PREFERENCE // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum From f6c8319361e5bf57867c3b4d7e030b0cd06405b7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 2 Feb 2017 10:41:19 +0000 Subject: [PATCH 90/97] Removed a number of static config pointers --- src/main/common/maths.h | 4 ++ src/main/config/config_master.h | 2 - src/main/drivers/light_led.c | 2 +- src/main/drivers/light_led.h | 5 ++- src/main/fc/config.c | 8 +--- src/main/fc/config.h | 4 ++ src/main/fc/fc_core.c | 11 ++--- src/main/fc/fc_init.c | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_controls.c | 10 ++--- src/main/fc/rc_controls.h | 13 +++++- src/main/flight/altitudehold.c | 60 +++++++++++--------------- src/main/flight/altitudehold.h | 8 +--- src/main/flight/failsafe.c | 7 +-- src/main/flight/failsafe.h | 2 +- src/main/flight/imu.h | 5 --- src/main/flight/mixer.h | 9 ---- src/main/flight/pid.h | 2 + src/main/sensors/battery.c | 19 +++----- src/main/sensors/battery.h | 6 +-- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- src/main/telemetry/frsky.c | 9 ++-- 23 files changed, 81 insertions(+), 113 deletions(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 78fd819d77..5283d6eba4 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -30,6 +30,10 @@ #define M_PIf 3.14159265358979323846f #define RAD (M_PIf / 180.0f) +#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) +#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) +#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) +#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) #define MIN(a, b) ((a) < (b) ? (a) : (b)) #define MAX(a, b) ((a) > (b) ? (a) : (b)) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 584c18876e..21cf9d4218 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -325,7 +325,5 @@ typedef struct master_s { } master_t; extern master_t masterConfig; -extern profile_t *currentProfile; -extern controlRateConfig_t *currentControlRateProfile; void createDefaultConfig(master_t *config); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 8011f04cf4..8f3fcd9d15 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -25,7 +25,7 @@ static IO_t leds[LED_NUMBER]; static uint8_t ledPolarity = 0; -void ledInit(statusLedConfig_t *statusLedConfig) +void ledInit(const statusLedConfig_t *statusLedConfig) { LED0_OFF; LED1_OFF; diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 25961579e4..341bbb81d9 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_NUMBER 3 @@ -26,6 +27,8 @@ typedef struct statusLedConfig_s { uint8_t polarity; } statusLedConfig_t; +PG_DECLARE(statusLedConfig_t, statusLedConfig); + // Helpful macros #ifdef LED0 # define LED0_TOGGLE ledToggle(0) @@ -57,6 +60,6 @@ typedef struct statusLedConfig_s { # define LED2_ON do {} while(0) #endif -void ledInit(statusLedConfig_t *statusLedConfig); +void ledInit(const statusLedConfig_t *statusLedConfig); void ledToggle(int led); void ledSet(int led, bool state); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3c17c4bd02..01a2af8dff 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -892,7 +892,6 @@ void activateConfig(void) useRcControlsConfig( modeActivationProfile()->modeActivationConditions, - &masterConfig.motorConfig, ¤tProfile->pidProfile ); @@ -918,12 +917,7 @@ void activateConfig(void) throttleCorrectionConfig()->throttle_correction_angle ); - configureAltitudeHold( - ¤tProfile->pidProfile, - &masterConfig.barometerConfig, - &masterConfig.rcControlsConfig, - &masterConfig.motorConfig - ); + configureAltitudeHold(¤tProfile->pidProfile); } void validateAndFixConfig(void) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 3b69a1f189..b092f15f80 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -66,6 +66,10 @@ typedef struct systemConfig_s { } systemConfig_t; //!!TODOPG_DECLARE(systemConfig_t, systemConfig); +struct profile_s; +extern struct profile_s *currentProfile; +struct controlRateConfig_s; +extern struct controlRateConfig_s *currentControlRateProfile; void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e32e3e7b9c..fb91d4d4ac 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -24,10 +24,10 @@ #include "blackbox/blackbox.h" -#include "common/maths.h" #include "common/axis.h" -#include "common/utils.h" #include "common/filter.h" +#include "common/maths.h" +#include "common/utils.h" #include "config/config_profile.h" #include "config/feature.h" @@ -72,6 +72,7 @@ #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" +#include "flight/imu.h" // June 2013 V2.2-dev @@ -296,7 +297,7 @@ void processRx(timeUs_t currentTimeUs) failsafeUpdateState(); } - throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset @@ -362,7 +363,7 @@ void processRx(timeUs_t currentTimeUs) } } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch); + processRcStickPositions(rxConfig(), throttleStatus, armingConfig()->disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); @@ -493,7 +494,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { - applyAltHold(&masterConfig.airplaneConfig); + applyAltHold(); } } #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index f1652946b6..5abdcaee74 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -443,7 +443,7 @@ void init(void) cliInit(serialConfig()); #endif - failsafeInit(flight3DConfig()->deadband3d_throttle); + failsafeInit(); rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 65ba91f0d8..495880d6f5 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1353,7 +1353,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationConditions(0), motorConfig(), ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile); } else { return MSP_RESULT_ERROR; } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d37278147e..ecbb07659d 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -131,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - updateCurrentMeter(ibatTimeSinceLastServiced, rxConfig(), flight3DConfig()->deadband3d_throttle); + updateCurrentMeter(ibatTimeSinceLastServiced); } } } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 943be00b87..9181a0a533 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -60,7 +60,6 @@ #include "flight/failsafe.h" -static const motorConfig_t *motorConfig; static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) @@ -116,13 +115,13 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -throttleStatus_e calculateThrottleStatus(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(void) { if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) + if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) return THROTTLE_LOW; } else { - if (rcData[THROTTLE] < rxConfig->mincheck) + if (rcData[THROTTLE] < rxConfig()->mincheck) return THROTTLE_LOW; } @@ -728,9 +727,8 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse) { - motorConfig = motorConfigToUse; pidProfile = pidProfileToUse; isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 4f2533afd1..928564daae 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -177,6 +177,15 @@ typedef struct rcControlsConfig_s { PG_DECLARE(rcControlsConfig_t, rcControlsConfig); +typedef struct flight3DConfig_s { + uint16_t deadband3d_low; // min 3d value + uint16_t deadband3d_high; // max 3d value + uint16_t neutral3d; // center 3d value + uint16_t deadband3d_throttle; // default throttle deadband from MIDRC +} flight3DConfig_t; + +PG_DECLARE(flight3DConfig_t, flight3DConfig); + typedef struct armingConfig_s { uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value @@ -189,7 +198,7 @@ bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); struct rxConfig_s; -throttleStatus_e calculateThrottleStatus(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +throttleStatus_e calculateThrottleStatus(void); void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); @@ -294,4 +303,4 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; struct motorConfig_s; -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 9689b24b69..d0157bd9eb 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -21,25 +21,26 @@ #include #include - #include "platform.h" + #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" -#include "sensors/barometer.h" -#include "sensors/sonar.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" -#include "fc/rc_controls.h" -#include "io/motors.h" - -#include "flight/pid.h" -#include "flight/imu.h" - -#include "fc/runtime_config.h" +#include "sensors/barometer.h" +#include "sensors/sonar.h" int32_t setVelocity = 0; @@ -50,22 +51,11 @@ int32_t AltHold; int32_t vario = 0; // variometer in cm/s -static barometerConfig_t *barometerConfig; static pidProfile_t *pidProfile; -static rcControlsConfig_t *rcControlsConfig; -static motorConfig_t *motorConfig; -void configureAltitudeHold( - pidProfile_t *initialPidProfile, - barometerConfig_t *intialBarometerConfig, - rcControlsConfig_t *initialRcControlsConfig, - motorConfig_t *initialMotorConfig -) +void configureAltitudeHold(pidProfile_t *initialPidProfile) { pidProfile = initialPidProfile; - barometerConfig = intialBarometerConfig; - rcControlsConfig = initialRcControlsConfig; - motorConfig = initialMotorConfig; } #if defined(BARO) || defined(SONAR) @@ -82,22 +72,22 @@ static void applyMultirotorAltHold(void) { static uint8_t isAltHoldChanged = 0; // multirotor alt hold - if (rcControlsConfig->alt_hold_fast_change) { + if (rcControlsConfig()->alt_hold_fast_change) { // rapid alt changes - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { errorVelocityI = 0; isAltHoldChanged = 1; - rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband; + rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband; } else { if (isAltHoldChanged) { AltHold = EstAlt; isAltHoldChanged = 0; } - rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle); + rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle); } } else { // slow alt changes, mostly used for aerial photography - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2; velocityControl = 1; @@ -107,23 +97,23 @@ static void applyMultirotorAltHold(void) velocityControl = 0; isAltHoldChanged = 0; } - rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle); + rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle); } } -static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig) +static void applyFixedWingAltHold(void) { // handle fixedwing-related althold. UNTESTED! and probably wrong // most likely need to check changes on pitch channel and 'reset' althold similar to // how throttle does it on multirotor - rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir; + rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir; } -void applyAltHold(airplaneConfig_t *airplaneConfig) +void applyAltHold(void) { if (STATE(FIXED_WING)) { - applyFixedWingAltHold(airplaneConfig); + applyFixedWingAltHold(); } else { applyMultirotorAltHold(); } @@ -272,7 +262,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) + accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; #ifdef DEBUG_ALT_HOLD @@ -308,7 +298,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay - vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel); + vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel); vel_tmp = lrintf(vel); // set vario diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3201f71b9a..8dceceb093 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -23,13 +23,9 @@ extern int32_t vario; void calculateEstimatedAltitude(timeUs_t currentTimeUs); struct pidProfile_s; -struct barometerConfig_s; -struct rcControlsConfig_s; -struct motorConfig_s; -void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig); +void configureAltitudeHold(struct pidProfile_s *initialPidProfile); -struct airplaneConfig_s; -void applyAltHold(struct airplaneConfig_s *airplaneConfig); +void applyAltHold(void); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2ef72459b1..ba604c309c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -53,8 +53,6 @@ static failsafeState_t failsafeState; -static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC - /* * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. */ @@ -71,9 +69,8 @@ void failsafeReset(void) failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } -void failsafeInit(uint16_t deadband3d_throttle) +void failsafeInit(void) { - deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; failsafeState.monitoring = false; @@ -180,7 +177,7 @@ void failsafeUpdateState(void) case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time - if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig(), deadband3dThrottle)) { + if (THROTTLE_HIGH == calculateThrottleStatus()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 2feefb297d..d11d61421a 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -74,7 +74,7 @@ typedef struct failsafeState_s { failsafeRxLinkState_e rxLinkState; } failsafeState_t; -void failsafeInit(uint16_t deadband3d_throttle); +void failsafeInit(void); void failsafeReset(void); void failsafeStartMonitoring(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 3b4d91ffbe..0deec6c89d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -32,11 +32,6 @@ extern float accVelScale; extern int32_t accSum[XYZ_AXIS_COUNT]; -#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) -#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) -#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) -#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) - typedef union { int16_t raw[XYZ_AXIS_COUNT]; struct { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 66baf8172b..86fd8f3d2c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -104,15 +104,6 @@ typedef struct mixerConfig_s { PG_DECLARE(mixerConfig_t, mixerConfig); -typedef struct flight3DConfig_s { - uint16_t deadband3d_low; // min 3d value - uint16_t deadband3d_high; // max 3d value - uint16_t neutral3d; // center 3d value - uint16_t deadband3d_throttle; // default throttle deadband from MIDRC -} flight3DConfig_t; - -PG_DECLARE(flight3DConfig_t, flight3DConfig); - 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 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d88a937a5b..1e093cdfc1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -93,6 +93,8 @@ typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate } pidConfig_t; +PG_DECLARE(pidConfig_t, pidConfig); + union rollAndPitchTrims_u; void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 02f53ffa93..e54a0a1ae3 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -22,29 +22,23 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/filter.h" +#include "common/utils.h" +#include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/system.h" -#include "fc/config.h" #include "fc/runtime_config.h" -#include "config/feature.h" +#include "io/beeper.h" #include "sensors/battery.h" #include "sensors/esc_sensor.h" -#include "fc/rc_controls.h" -#include "io/beeper.h" - -#include "rx/rx.h" - -#include "common/utils.h" #define VBAT_LPF_FREQ 0.4f #define IBAT_LPF_FREQ 0.4f @@ -64,7 +58,7 @@ uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatest = 0; // most recent unsmoothed value int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) -int32_t amperageLatest = 0; // most recent value +int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -266,9 +260,8 @@ void updateConsumptionWarning(void) } } -void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void updateCurrentMeter(int32_t lastUpdateAt) { - (void)(rxConfig); if (getBatteryState() != BATTERY_NOT_PRESENT) { switch(batteryConfig()->currentMeterType) { case CURRENT_SENSOR_ADC: @@ -279,7 +272,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16 case CURRENT_SENSOR_VIRTUAL: amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index a8e21145ff..91b59568ce 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -18,7 +18,6 @@ #pragma once #include "config/parameter_group.h" -#include "common/maths.h" // for fix12_t #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 @@ -80,9 +79,6 @@ extern uint16_t batteryWarningVoltage; extern int32_t amperageLatest; extern int32_t amperage; extern int32_t mAhDrawn; -#ifndef USE_PARAMETER_GROUPS -extern const batteryConfig_t *batteryConfig; -#endif batteryState_e getBatteryState(void); const char * getBatteryStateString(void); @@ -90,7 +86,7 @@ void updateBattery(void); void batteryInit(void); struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void updateCurrentMeter(int32_t lastUpdateAt); int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 4912bc15f8..550f69b97c 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1065,7 +1065,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); } else { ret = BST_FAILED; } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index b764353caa..ef1877e82e 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -195,18 +195,15 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(void) { sendDataHead(ID_RPM); #ifdef USE_ESC_SENSOR - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0); #else if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; @@ -538,7 +535,7 @@ void handleFrSkyTelemetry(void) if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(rxConfig(), flight3DConfig()->deadband3d_throttle); + sendThrottleOrBatterySizeAsRpm(); if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) { sendVoltage(); From becdeb4c9be1413716507c5f2bcfd20d2bcbd167 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 2 Feb 2017 10:50:59 +0000 Subject: [PATCH 91/97] Used PGs rather than parameters for processRcStickPositions --- src/main/fc/fc_core.c | 2 +- src/main/fc/rc_controls.c | 12 ++++++------ src/main/fc/rc_controls.h | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index fb91d4d4ac..270ee1b595 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -363,7 +363,7 @@ void processRx(timeUs_t currentTimeUs) } } - processRcStickPositions(rxConfig(), throttleStatus, armingConfig()->disarm_kill_switch); + processRcStickPositions(throttleStatus); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 9181a0a533..4ce3c96d1b 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -128,7 +128,7 @@ throttleStatus_e calculateThrottleStatus(void) return THROTTLE_HIGH; } -void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) +void processRcStickPositions(throttleStatus_e throttleStatus) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -140,9 +140,9 @@ void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e thrott // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; - if (rcData[i] > rxConfig->mincheck) + if (rcData[i] > rxConfig()->mincheck) stTmp |= 0x80; // check for MIN - if (rcData[i] < rxConfig->maxcheck) + if (rcData[i] < rxConfig()->maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { @@ -169,7 +169,7 @@ void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e thrott if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { - if (disarm_kill_switch) { + if (armingConfig()->disarm_kill_switch) { mwDisarm(); } else if (throttleStatus == THROTTLE_LOW) { mwDisarm(); @@ -681,9 +681,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { int delta; - if (rcData[channelIndex] > rxConfig->midrc + 200) { + if (rcData[channelIndex] > rxConfig()->midrc + 200) { delta = adjustmentState->config->data.stepConfig.step; - } else if (rcData[channelIndex] < rxConfig->midrc - 200) { + } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { delta = 0 - adjustmentState->config->data.stepConfig.step; } else { // returning the switch to the middle immediately resets the ready state diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 928564daae..864c4ec43e 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -197,9 +197,8 @@ PG_DECLARE(armingConfig_t, armingConfig); bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -struct rxConfig_s; throttleStatus_e calculateThrottleStatus(void); -void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +void processRcStickPositions(throttleStatus_e throttleStatus); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -295,6 +294,7 @@ typedef struct adjustmentProfile_s { bool isAirmodeActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); +struct rxConfig_s; void processRcAdjustments(controlRateConfig_t *controlRateConfig, const struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); From f858cd9d380486d1269f2482bc51850d166c8cac Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 2 Feb 2017 11:23:39 +0000 Subject: [PATCH 92/97] Removed unused parameter to processRcAdjustments --- src/main/fc/fc_core.c | 2 +- src/main/fc/rc_controls.c | 2 +- src/main/fc/rc_controls.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 270ee1b595..1a3197ce59 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -373,7 +373,7 @@ void processRx(timeUs_t currentTimeUs) if (!cliMode) { updateAdjustmentStates(adjustmentProfile()->adjustmentRanges); - processRcAdjustments(currentControlRateProfile, rxConfig()); + processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 4ce3c96d1b..2f09eed281 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -646,7 +646,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) #define RESET_FREQUENCY_2HZ (1000 / 2) -void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig) +void processRcAdjustments(controlRateConfig_t *controlRateConfig) { uint8_t adjustmentIndex; uint32_t now = millis(); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 864c4ec43e..25d1546e71 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -295,7 +295,7 @@ bool isAirmodeActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); struct rxConfig_s; -void processRcAdjustments(controlRateConfig_t *controlRateConfig, const struct rxConfig_s *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig); bool isUsingSticksForArming(void); From 8eead7d1aa909b590ccf1cf1f90fade0f80ade5a Mon Sep 17 00:00:00 2001 From: Szpero Date: Fri, 3 Feb 2017 02:46:13 +0100 Subject: [PATCH 93/97] Update target.c Fix for non working DSHOT. Old file not maping motors to DMA channels so DSHOT was unable to work. Still motor 1 need to be remaped to PPM PA7. --- src/main/target/AIR32/target.c | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index a6b455f81b..4ea67b44e6 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -21,17 +21,18 @@ #include "drivers/io.h" #include "drivers/timer.h" +#include "drivers/timer_def.h" #include "drivers/dma.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM1 - PA4 - *TIM3_CH2 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_2, NULL, 0 }, // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_9, NULL, 0 }, // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_6, NULL, 0 }, // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_1, NULL, 0 }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM_USE_LED, 0, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - *TIM3_CH2 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; From 0d931c7f510e258d37d5f1846621f03932ef4a0c Mon Sep 17 00:00:00 2001 From: Szpero Date: Fri, 3 Feb 2017 03:40:29 +0100 Subject: [PATCH 94/97] Update target.c --- src/main/target/AIR32/target.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 4ea67b44e6..d39e00200e 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -26,13 +26,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, 1), // PWM1 - PA4 - *TIM3_CH2 - DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR - DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, 1), // PWM2 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1), // PWM3 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1), // PWM4 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, 1), // PWM5 - PA1 - *TIM2_CH2, TIM15_CH1N + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 1), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 + DEF_TIM(TIM15, CH2, PA3, TIM_USE_MOTOR, 1), // PWM7 - PA3 - *TIM15_CH2, TIM2_CH4 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, 1), // PWM8 - PA8 - *TIM1_CH1, TIM4_ETR + DEF_TIM(TIM17, CH1, PA7, TIM_USE_PPM, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_LED, 0), // PPM - PA7 - *TIM17_CH1, TIM1_CH1N, TIM8_CH1 }; From f2e5c7d0b4dffc6eb11a66531919296effd716fe Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 3 Feb 2017 10:49:02 +0000 Subject: [PATCH 95/97] Split RC adjustments into separate module --- Makefile | 1 + src/main/config/config_master.h | 1 + src/main/fc/config.c | 6 +- src/main/fc/rc_adjustments.c | 456 ++++++++++++++++++++++++++++++++ src/main/fc/rc_adjustments.h | 116 ++++++++ src/main/fc/rc_controls.c | 401 ---------------------------- src/main/fc/rc_controls.h | 93 ------- 7 files changed, 576 insertions(+), 498 deletions(-) create mode 100644 src/main/fc/rc_adjustments.c create mode 100644 src/main/fc/rc_adjustments.h diff --git a/Makefile b/Makefile index 898b6f42f7..8b8183ba4a 100644 --- a/Makefile +++ b/Makefile @@ -594,6 +594,7 @@ COMMON_SRC = \ fc/fc_rc.c \ fc/fc_msp.c \ fc/fc_tasks.c \ + fc/rc_adjustments.c \ fc/rc_controls.c \ fc/runtime_config.c \ fc/cli.c \ diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 21cf9d4218..7f75ffb4b4 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -35,6 +35,7 @@ #include "drivers/flash.h" #include "drivers/display.h" +#include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "flight/failsafe.h" diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e4eb8c3c10..86b2300b5c 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -890,10 +890,8 @@ void activateConfig(void) { resetAdjustmentStates(); - useRcControlsConfig( - modeActivationProfile()->modeActivationConditions, - ¤tProfile->pidProfile - ); + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); + useAdjustmentConfig(¤tProfile->pidProfile); #ifdef GPS gpsUseProfile(&masterConfig.gpsProfile); diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c new file mode 100644 index 0000000000..69c04174b9 --- /dev/null +++ b/src/main/fc/rc_adjustments.c @@ -0,0 +1,456 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "platform.h" + +#include "blackbox/blackbox.h" + +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/system.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "config/feature.h" +#include "config/config_master.h" + +#include "flight/pid.h" + +#include "io/beeper.h" +#include "io/motors.h" + +#include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "fc/config.h" + +#include "rx/rx.h" + +static pidProfile_t *pidProfile; + +static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) +{ +#ifndef BLACKBOX + UNUSED(adjustmentFunction); + UNUSED(newValue); +#else + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_inflightAdjustment_t eventData; + eventData.adjustmentFunction = adjustmentFunction; + eventData.newValue = newValue; + eventData.floatFlag = false; + blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); + } +#endif +} + +#if 0 +static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) +{ +#ifndef BLACKBOX + UNUSED(adjustmentFunction); + UNUSED(newFloatValue); +#else + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_inflightAdjustment_t eventData; + eventData.adjustmentFunction = adjustmentFunction; + eventData.newFloatValue = newFloatValue; + eventData.floatFlag = true; + blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); + } +#endif +} +#endif + +static uint8_t adjustmentStateMask = 0; + +#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex) +#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex) + +#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) + +// sync with adjustmentFunction_e +static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { + { + .adjustmentFunction = ADJUSTMENT_RC_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RC_EXPO, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, + .mode = ADJUSTMENT_MODE_SELECT, + .data = { .selectConfig = { .switchPositions = 3 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_D_SETPOINT, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + } +}; + +#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 + +static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; + +static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) +{ + adjustmentState_t *adjustmentState = &adjustmentStates[index]; + + if (adjustmentState->config == adjustmentConfig) { + // already configured + return; + } + adjustmentState->auxChannelIndex = auxSwitchChannelIndex; + adjustmentState->config = adjustmentConfig; + adjustmentState->timeoutAt = 0; + + MARK_ADJUSTMENT_FUNCTION_AS_READY(index); +} + +static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) +{ + int newValue; + + if (delta > 0) { + beeperConfirmationBeeps(2); + } else { + beeperConfirmationBeeps(1); + } + switch(adjustmentFunction) { + case ADJUSTMENT_RC_RATE: + newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); + break; + case ADJUSTMENT_RC_EXPO: + newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcExpo8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); + break; + case ADJUSTMENT_THROTTLE_EXPO: + newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c + controlRateConfig->thrExpo8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_RATE: + case ADJUSTMENT_PITCH_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_PITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); + if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE + case ADJUSTMENT_ROLL_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->rates[FD_ROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); + break; + case ADJUSTMENT_YAW_RATE: + newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + controlRateConfig->rates[FD_YAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_P: + case ADJUSTMENT_PITCH_P: + newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_P) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_P + case ADJUSTMENT_ROLL_P: + newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_I: + case ADJUSTMENT_PITCH_I: + newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_I) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_I + case ADJUSTMENT_ROLL_I: + newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); + break; + case ADJUSTMENT_PITCH_ROLL_D: + case ADJUSTMENT_PITCH_D: + newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDPITCH] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); + + if (adjustmentFunction == ADJUSTMENT_PITCH_D) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_D + case ADJUSTMENT_ROLL_D: + newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDROLL] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); + break; + case ADJUSTMENT_YAW_P: + newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->P8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); + break; + case ADJUSTMENT_YAW_I: + newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->I8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); + break; + case ADJUSTMENT_YAW_D: + newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c + pidProfile->D8[PIDYAW] = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); + break; + case ADJUSTMENT_RC_RATE_YAW: + newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c + controlRateConfig->rcYawRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); + break; + case ADJUSTMENT_D_SETPOINT: + newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c + pidProfile->dtermSetpointWeight = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); + break; + case ADJUSTMENT_D_SETPOINT_TRANSITION: + newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c + pidProfile->setpointRelaxRatio = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); + break; + default: + break; + }; +} + +static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) +{ + bool applied = false; + + switch(adjustmentFunction) { + case ADJUSTMENT_RATE_PROFILE: + if (getCurrentControlRateProfile() != position) { + changeControlRateProfile(position); + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); + applied = true; + } + break; + } + + if (applied) { + beeperConfirmationBeeps(position + 1); + } +} + +#define RESET_FREQUENCY_2HZ (1000 / 2) + +void processRcAdjustments(controlRateConfig_t *controlRateConfig) +{ + const uint32_t now = millis(); + + const bool canUseRxData = rxIsReceivingSignal(); + + for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { + adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex]; + + if (!adjustmentState->config) { + continue; + } + const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction; + if (adjustmentFunction == ADJUSTMENT_NONE) { + continue; + } + + const int32_t signedDiff = now - adjustmentState->timeoutAt; + const bool canResetReadyStates = signedDiff >= 0L; + + if (canResetReadyStates) { + adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; + MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); + } + + if (!canUseRxData) { + continue; + } + + const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex; + + if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { + int delta; + if (rcData[channelIndex] > rxConfig()->midrc + 200) { + delta = adjustmentState->config->data.stepConfig.step; + } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { + delta = 0 - adjustmentState->config->data.stepConfig.step; + } else { + // returning the switch to the middle immediately resets the ready state + MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); + adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; + continue; + } + if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) { + continue; + } + + applyStepAdjustment(controlRateConfig,adjustmentFunction,delta); + pidInitConfig(pidProfile); + } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { + const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); + const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; + applySelectAdjustment(adjustmentFunction, position); + } + MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex); + } +} + +void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) +{ + for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { + adjustmentRange_t *adjustmentRange = &adjustmentRanges[index]; + if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { + const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; + configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); + } + } +} + +void resetAdjustmentStates(void) +{ + memset(adjustmentStates, 0, sizeof(adjustmentStates)); +} + +void useAdjustmentConfig(pidProfile_t *pidProfileToUse) +{ + pidProfile = pidProfileToUse; +} diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h new file mode 100644 index 0000000000..ad3fc47451 --- /dev/null +++ b/src/main/fc/rc_adjustments.h @@ -0,0 +1,116 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include "config/parameter_group.h" +#include "fc/rc_controls.h" + +typedef enum { + ADJUSTMENT_NONE = 0, + ADJUSTMENT_RC_RATE, + ADJUSTMENT_RC_EXPO, + ADJUSTMENT_THROTTLE_EXPO, + ADJUSTMENT_PITCH_ROLL_RATE, + ADJUSTMENT_YAW_RATE, + ADJUSTMENT_PITCH_ROLL_P, + ADJUSTMENT_PITCH_ROLL_I, + ADJUSTMENT_PITCH_ROLL_D, + ADJUSTMENT_YAW_P, + ADJUSTMENT_YAW_I, + ADJUSTMENT_YAW_D, + ADJUSTMENT_RATE_PROFILE, + ADJUSTMENT_PITCH_RATE, + ADJUSTMENT_ROLL_RATE, + ADJUSTMENT_PITCH_P, + ADJUSTMENT_PITCH_I, + ADJUSTMENT_PITCH_D, + ADJUSTMENT_ROLL_P, + ADJUSTMENT_ROLL_I, + ADJUSTMENT_ROLL_D, + ADJUSTMENT_RC_RATE_YAW, + ADJUSTMENT_D_SETPOINT, + ADJUSTMENT_D_SETPOINT_TRANSITION, + ADJUSTMENT_FUNCTION_COUNT +} adjustmentFunction_e; + + +typedef enum { + ADJUSTMENT_MODE_STEP, + ADJUSTMENT_MODE_SELECT +} adjustmentMode_e; + +typedef struct adjustmentStepConfig_s { + uint8_t step; +} adjustmentStepConfig_t; + +typedef struct adjustmentSelectConfig_s { + uint8_t switchPositions; +} adjustmentSelectConfig_t; + +typedef union adjustmentConfig_u { + adjustmentStepConfig_t stepConfig; + adjustmentSelectConfig_t selectConfig; +} adjustmentData_t; + +typedef struct adjustmentConfig_s { + uint8_t adjustmentFunction; + uint8_t mode; + adjustmentData_t data; +} adjustmentConfig_t; + +typedef struct adjustmentRange_s { + // when aux channel is in range... + uint8_t auxChannelIndex; + channelRange_t range; + + // ..then apply the adjustment function to the auxSwitchChannel ... + uint8_t adjustmentFunction; + uint8_t auxSwitchChannelIndex; + + // ... via slot + uint8_t adjustmentIndex; +} adjustmentRange_t; + +#define ADJUSTMENT_INDEX_OFFSET 1 + +typedef struct adjustmentState_s { + uint8_t auxChannelIndex; + const adjustmentConfig_t *config; + uint32_t timeoutAt; +} adjustmentState_t; + + +#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel +#endif + +#define MAX_ADJUSTMENT_RANGE_COUNT 15 + +PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); + +typedef struct adjustmentProfile_s { + adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; +} adjustmentProfile_t; + +void resetAdjustmentStates(void); +void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); +struct controlRateConfig_s; +void processRcAdjustments(struct controlRateConfig_s *controlRateConfig); +struct pidProfile_s; +void useAdjustmentConfig(struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 2f09eed281..d6108b5482 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -74,37 +74,6 @@ bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } -void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { -#ifndef BLACKBOX -#define UNUSED(x) (void)(x) - UNUSED(adjustmentFunction); - UNUSED(newValue); -#else - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_inflightAdjustment_t eventData; - eventData.adjustmentFunction = adjustmentFunction; - eventData.newValue = newValue; - eventData.floatFlag = false; - blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); - } -#endif -} - -void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) { -#ifndef BLACKBOX - UNUSED(adjustmentFunction); - UNUSED(newFloatValue); -#else - if (feature(FEATURE_BLACKBOX)) { - flightLogEvent_inflightAdjustment_t eventData; - eventData.adjustmentFunction = adjustmentFunction; - eventData.newFloatValue = newFloatValue; - eventData.floatFlag = true; - blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData); - } -#endif -} - bool isUsingSticksForArming(void) { return isUsingSticksToArm; @@ -358,371 +327,6 @@ void updateActivatedModes(modeActivationCondition_t *modeActivationConditions) } } -uint8_t adjustmentStateMask = 0; - -#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex) -#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex) - -#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) - -// sync with adjustmentFunction_e -static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { - { - .adjustmentFunction = ADJUSTMENT_RC_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_RC_EXPO, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_YAW_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, - .mode = ADJUSTMENT_MODE_SELECT, - .data = { .selectConfig = { .switchPositions = 3 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_PITCH_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_P, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_I, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_ROLL_D, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_D_SETPOINT, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - }, - { - .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION, - .mode = ADJUSTMENT_MODE_STEP, - .data = { .stepConfig = { .step = 1 }} - } -}; - -#define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 - -adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; - -static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) { - adjustmentState_t *adjustmentState = &adjustmentStates[index]; - - if (adjustmentState->config == adjustmentConfig) { - // already configured - return; - } - adjustmentState->auxChannelIndex = auxSwitchChannelIndex; - adjustmentState->config = adjustmentConfig; - adjustmentState->timeoutAt = 0; - - MARK_ADJUSTMENT_FUNCTION_AS_READY(index); -} - -static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { - int newValue; - - if (delta > 0) { - beeperConfirmationBeeps(2); - } else { - beeperConfirmationBeeps(1); - } - switch(adjustmentFunction) { - case ADJUSTMENT_RC_RATE: - newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcRate8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue); - break; - case ADJUSTMENT_RC_EXPO: - newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); - break; - case ADJUSTMENT_THROTTLE_EXPO: - newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c - controlRateConfig->thrExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_RATE: - case ADJUSTMENT_PITCH_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_PITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); - if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE - case ADJUSTMENT_ROLL_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_ROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); - break; - case ADJUSTMENT_YAW_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); - controlRateConfig->rates[FD_YAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_P: - case ADJUSTMENT_PITCH_P: - newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_P) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_P - case ADJUSTMENT_ROLL_P: - newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_I: - case ADJUSTMENT_PITCH_I: - newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_I) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_I - case ADJUSTMENT_ROLL_I: - newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); - break; - case ADJUSTMENT_PITCH_ROLL_D: - case ADJUSTMENT_PITCH_D: - newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDPITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); - - if (adjustmentFunction == ADJUSTMENT_PITCH_D) { - break; - } - // follow though for combined ADJUSTMENT_PITCH_ROLL_D - case ADJUSTMENT_ROLL_D: - newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); - break; - case ADJUSTMENT_YAW_P: - newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->P8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); - break; - case ADJUSTMENT_YAW_I: - newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->I8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); - break; - case ADJUSTMENT_YAW_D: - newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c - pidProfile->D8[PIDYAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); - break; - case ADJUSTMENT_RC_RATE_YAW: - newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c - controlRateConfig->rcYawRate8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); - break; - case ADJUSTMENT_D_SETPOINT: - newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c - pidProfile->dtermSetpointWeight = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); - break; - case ADJUSTMENT_D_SETPOINT_TRANSITION: - newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c - pidProfile->setpointRelaxRatio = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); - break; - default: - break; - }; -} - -static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) -{ - bool applied = false; - - switch(adjustmentFunction) { - case ADJUSTMENT_RATE_PROFILE: - if (getCurrentControlRateProfile() != position) { - changeControlRateProfile(position); - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); - applied = true; - } - break; - } - - if (applied) { - beeperConfirmationBeeps(position + 1); - } -} - -#define RESET_FREQUENCY_2HZ (1000 / 2) - -void processRcAdjustments(controlRateConfig_t *controlRateConfig) -{ - uint8_t adjustmentIndex; - uint32_t now = millis(); - - bool canUseRxData = rxIsReceivingSignal(); - - - for (adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { - adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex]; - - if (!adjustmentState->config) { - continue; - } - uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction; - if (adjustmentFunction == ADJUSTMENT_NONE) { - continue; - } - - int32_t signedDiff = now - adjustmentState->timeoutAt; - bool canResetReadyStates = signedDiff >= 0L; - - if (canResetReadyStates) { - adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; - MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); - } - - if (!canUseRxData) { - continue; - } - - uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex; - - if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { - int delta; - if (rcData[channelIndex] > rxConfig()->midrc + 200) { - delta = adjustmentState->config->data.stepConfig.step; - } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { - delta = 0 - adjustmentState->config->data.stepConfig.step; - } else { - // returning the switch to the middle immediately resets the ready state - MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); - adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; - continue; - } - if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) { - continue; - } - - applyStepAdjustment(controlRateConfig,adjustmentFunction,delta); - pidInitConfig(pidProfile); - } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { - uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); - uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; - - applySelectAdjustment(adjustmentFunction, position); - } - MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex); - } -} - -void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) -{ - uint8_t index; - - for (index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { - adjustmentRange_t *adjustmentRange = &adjustmentRanges[index]; - - if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { - - const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; - - configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); - } - } -} - int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } @@ -733,8 +337,3 @@ void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditio isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); } - -void resetAdjustmentStates(void) -{ - memset(adjustmentStates, 0, sizeof(adjustmentStates)); -} diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 24024da319..ce92e6b477 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -204,104 +204,11 @@ void processRcStickPositions(throttleStatus_e throttleStatus); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); - -typedef enum { - ADJUSTMENT_NONE = 0, - ADJUSTMENT_RC_RATE, - ADJUSTMENT_RC_EXPO, - ADJUSTMENT_THROTTLE_EXPO, - ADJUSTMENT_PITCH_ROLL_RATE, - ADJUSTMENT_YAW_RATE, - ADJUSTMENT_PITCH_ROLL_P, - ADJUSTMENT_PITCH_ROLL_I, - ADJUSTMENT_PITCH_ROLL_D, - ADJUSTMENT_YAW_P, - ADJUSTMENT_YAW_I, - ADJUSTMENT_YAW_D, - ADJUSTMENT_RATE_PROFILE, - ADJUSTMENT_PITCH_RATE, - ADJUSTMENT_ROLL_RATE, - ADJUSTMENT_PITCH_P, - ADJUSTMENT_PITCH_I, - ADJUSTMENT_PITCH_D, - ADJUSTMENT_ROLL_P, - ADJUSTMENT_ROLL_I, - ADJUSTMENT_ROLL_D, - ADJUSTMENT_RC_RATE_YAW, - ADJUSTMENT_D_SETPOINT, - ADJUSTMENT_D_SETPOINT_TRANSITION, - ADJUSTMENT_FUNCTION_COUNT -} adjustmentFunction_e; - - -typedef enum { - ADJUSTMENT_MODE_STEP, - ADJUSTMENT_MODE_SELECT -} adjustmentMode_e; - -typedef struct adjustmentStepConfig_s { - uint8_t step; -} adjustmentStepConfig_t; - -typedef struct adjustmentSelectConfig_s { - uint8_t switchPositions; -} adjustmentSelectConfig_t; - -typedef union adjustmentConfig_u { - adjustmentStepConfig_t stepConfig; - adjustmentSelectConfig_t selectConfig; -} adjustmentData_t; - -typedef struct adjustmentConfig_s { - uint8_t adjustmentFunction; - uint8_t mode; - adjustmentData_t data; -} adjustmentConfig_t; - -typedef struct adjustmentRange_s { - // when aux channel is in range... - uint8_t auxChannelIndex; - channelRange_t range; - - // ..then apply the adjustment function to the auxSwitchChannel ... - uint8_t adjustmentFunction; - uint8_t auxSwitchChannelIndex; - - // ... via slot - uint8_t adjustmentIndex; -} adjustmentRange_t; - -#define ADJUSTMENT_INDEX_OFFSET 1 - -typedef struct adjustmentState_s { - uint8_t auxChannelIndex; - const adjustmentConfig_t *config; - uint32_t timeoutAt; -} adjustmentState_t; - - -#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT -#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel -#endif - -#define MAX_ADJUSTMENT_RANGE_COUNT 15 - -PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); - -typedef struct adjustmentProfile_s { - adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; -} adjustmentProfile_t; - bool isAirmodeActive(void); -void resetAdjustmentStates(void); -void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -struct rxConfig_s; -void processRcAdjustments(controlRateConfig_t *controlRateConfig); bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; -struct motorConfig_s; void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse); From c72c4ca2e86ac2b1b32a7ce6592bbce70b8fe4ca Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 4 Feb 2017 10:04:12 +0100 Subject: [PATCH 96/97] Remove unused SENSORSET definition --- src/main/target/AIR32/target.h | 3 --- src/main/target/ANYFCF7/target.h | 2 -- src/main/target/COLIBRI/target.h | 2 -- src/main/target/FURYF7/target.h | 2 -- src/main/target/MOTOLAB/target.h | 2 -- src/main/target/NUCLEOF7/target.h | 2 -- src/main/target/OMNIBUSF4/target.h | 2 -- src/main/target/PIKOBLX/target.h | 2 -- src/main/target/RCEXPLORERF3/target.h | 2 -- src/main/target/REVO/target.h | 2 -- 10 files changed, 21 deletions(-) diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index eb848b4532..8b873e6358 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -85,9 +85,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) - #undef GPS #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index ab0fa2ada6..7173268a47 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -135,8 +135,6 @@ #define I2C_DEVICE (I2CDEV_4) //#define I2C_DEVICE_EXT (I2CDEV_2) -#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) - #define USE_ADC #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index cfd8ef165a..71f4ce0b08 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -117,8 +117,6 @@ #define I2C3_SCL PA8 #define I2C3_SDA PC9 -#define SENSORS_SET (SENSOR_ACC) - #define LED_STRIP // alternative defaults for Colibri/Gemini target diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 2cbffeaedf..2206a57e6a 100644 --- a/src/main/target/FURYF7/target.h +++ b/src/main/target/FURYF7/target.h @@ -134,8 +134,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 62a4de9952..4d429b641b 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -84,8 +84,6 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define SENSORS_SET (SENSOR_ACC) - #undef GPS #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h index a1cf620a80..c910194056 100644 --- a/src/main/target/NUCLEOF7/target.h +++ b/src/main/target/NUCLEOF7/target.h @@ -136,8 +136,6 @@ //#define I2C_DEVICE_EXT (I2CDEV_2) -#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) - #define USE_ADC #define VBAT_ADC_PIN PA3 #define CURRENT_METER_ADC_PIN PC0 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index e97ff6468b..0ad2aff3b4 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -164,8 +164,6 @@ #define LED_STRIP -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_VBAT) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index a55e4eca26..9e8357581f 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -68,8 +68,6 @@ #define USE_SPI #define USE_SPI_DEVICE_2 -#define SENSORS_SET (SENSOR_ACC) - #define TELEMETRY #define BLACKBOX #define SERIAL_RX diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h index f01f07a096..dcb58933f8 100644 --- a/src/main/target/RCEXPLORERF3/target.h +++ b/src/main/target/RCEXPLORERF3/target.h @@ -92,8 +92,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) - #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index a93fdc3813..f5ad7a5a79 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -186,8 +186,6 @@ #define LED_STRIP -#define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #if defined(PODIUMF4) #define SERIALRX_PROVIDER SERIALRX_SBUS From b7605f1d8f2cfe95c6c26f5cb8caef16fb8a816d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 7 Feb 2017 14:05:36 +0100 Subject: [PATCH 97/97] Fix merge artifacts --- src/main/fc/rc_adjustments.c | 3 +- src/main/fc/rc_curves.c | 60 --------------------------- src/main/fc/rc_curves.h | 25 ----------- src/main/target/BETAFLIGHTF3/config.c | 18 +------- 4 files changed, 4 insertions(+), 102 deletions(-) delete mode 100644 src/main/fc/rc_curves.c delete mode 100644 src/main/fc/rc_curves.h diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 69c04174b9..62669bb66e 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -45,7 +45,7 @@ #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "fc/rc_curves.h" +#include "fc/fc_rc.h" #include "fc/config.h" #include "rx/rx.h" @@ -254,6 +254,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_THROTTLE_EXPO: newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c controlRateConfig->thrExpo8 = newValue; + generateThrottleCurve(); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; case ADJUSTMENT_PITCH_ROLL_RATE: diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c deleted file mode 100644 index 84b437b221..0000000000 --- a/src/main/fc/rc_curves.c +++ /dev/null @@ -1,60 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#include "config/feature.h" - -#include "io/motors.h" - -#include "fc/config.h" -#include "fc/rc_curves.h" -#include "fc/rc_controls.h" - -#include "rx/rx.h" - - -#define THROTTLE_LOOKUP_LENGTH 12 -static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE - -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig) -{ - uint8_t i; - uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle); - - for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - int16_t tmp = 10 * i - controlRateConfig->thrMid8; - uint8_t y = 1; - if (tmp > 0) - y = 100 - controlRateConfig->thrMid8; - if (tmp < 0) - y = controlRateConfig->thrMid8; - lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = minThrottle + (int32_t) (motorConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] - } -} - -int16_t rcLookupThrottle(int32_t tmp) -{ - const int32_t tmp2 = tmp / 100; - // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] - return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; -} - diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h deleted file mode 100644 index c13851969e..0000000000 --- a/src/main/fc/rc_curves.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -struct controlRateConfig_s; -struct motorConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig); - -int16_t rcLookupThrottle(int32_t tmp); - diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index b65b136e04..e7b417c39e 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -20,25 +20,11 @@ #include -#include "common/utils.h" - -#include "drivers/io.h" - -#include "fc/rc_controls.h" - -#include "flight/failsafe.h" -#include "flight/mixer.h" -#include "flight/pid.h" - -#include "rx/rx.h" - -#include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" void targetConfiguration(master_t *config) { - UNUSED(config); - - batteryConfig->currentMeterScale = 235; + config->batteryConfig.currentMeterScale = 235; }