From fc723121b23a4f5329a48a31e4afdf5dc2547443 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 24 Jan 2018 20:27:14 +0100 Subject: [PATCH] group controlRateConfig settings by function --- src/main/blackbox/blackbox.c | 18 +++---- src/main/cms/cms_menu_imu.c | 4 +- src/main/fc/controlrate_profile.c | 38 +++++++++------ src/main/fc/controlrate_profile.h | 30 ++++++++---- src/main/fc/fc_core.c | 12 ++--- src/main/fc/fc_msp.c | 30 ++++++------ src/main/fc/rc_adjustments.c | 20 ++++---- src/main/fc/rc_curves.c | 10 ++-- src/main/fc/settings.yaml | 55 +++++++++++----------- src/main/flight/failsafe.c | 2 +- src/main/flight/pid.c | 24 +++++----- src/main/flight/pid_autotune.c | 2 +- src/main/navigation/navigation_fixedwing.c | 2 +- src/main/target/FALCORE/config.c | 18 +++---- 14 files changed, 142 insertions(+), 123 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 597b8ca4510..0b9f018da8b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1383,15 +1383,15 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getPidUpdateRate()); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyroSyncDenominator); BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", 100); //For compatibility reasons write rc_rate 100 - BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8); - BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->rcYawExpo8); - BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); - BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); - BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); - BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint); - BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], - currentControlRateProfile->rates[PITCH], - currentControlRateProfile->rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8); + BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8); + BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->throttle.rcMid8); + BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->throttle.rcExpo8); + BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->throttle.dynPID); + BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->throttle.pa_breakpoint); + BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL], + currentControlRateProfile->stabilized.rates[PITCH], + currentControlRateProfile->stabilized.rates[YAW]); BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", pidBank()->pid[PID_ROLL].P, pidBank()->pid[PID_ROLL].I, pidBank()->pid[PID_ROLL].D); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index b001357c9e8..632aaba8513 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -270,8 +270,8 @@ static OSD_Entry cmsx_menuManualRateProfileEntries[] = OSD_SETTING_ENTRY("MANU PITCH RATE", SETTING_MANUAL_PITCH_RATE), OSD_SETTING_ENTRY("MANU YAW RATE", SETTING_MANUAL_YAW_RATE), - OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_RC_MANUAL_EXPO), - OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_RC_MANUAL_YAW_EXPO), + OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_MANUAL_RC_EXPO), + OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_MANUAL_RC_YAW_EXPO), { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index d9eb6e599ee..68cae859368 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -33,26 +33,34 @@ const controlRateConfig_t *currentControlRateProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 1); void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) { for (int i = 0; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { RESET_CONFIG(controlRateConfig_t, &instance[i], - .rcExpo8 = 70, - .manual_rcExpo8 = 70, - .thrMid8 = 50, - .thrExpo8 = 0, - .dynThrPID = 0, - .rcYawExpo8 = 20, - .manual_rcYawExpo8 = 20, - .tpa_breakpoint = 1500, - .rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, - .rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, - .rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT, - .manual_rates[FD_ROLL] = 100, - .manual_rates[FD_PITCH] = 100, - .manual_rates[FD_YAW] = 100 + .throttle = { + .rcMid8 = 50, + .rcExpo8 = 0, + .dynPID = 0, + .pa_breakpoint = 1500 + }, + + .stabilized = { + .rcExpo8 = 70, + .rcYawExpo8 = 20, + .rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, + .rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, + .rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT, + }, + + .manual = { + .rcExpo8 = 70, + .rcYawExpo8 = 20, + .rates[FD_ROLL] = 100, + .rates[FD_PITCH] = 100, + .rates[FD_YAW] = 100 + } ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 581897ef654..15cd2f1aea9 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -41,16 +41,26 @@ and so on. #define CONTROL_RATE_CONFIG_TPA_MAX 100 typedef struct controlRateConfig_s { - uint16_t tpa_breakpoint; // Breakpoint where TPA is activated - uint8_t rcExpo8; - uint8_t manual_rcExpo8; - uint8_t thrMid8; - uint8_t thrExpo8; - uint8_t rates[3]; - uint8_t manual_rates[3]; - uint8_t dynThrPID; - uint8_t rcYawExpo8; - uint8_t manual_rcYawExpo8; + + struct { + uint8_t rcMid8; + uint8_t rcExpo8; + uint8_t dynPID; + uint16_t pa_breakpoint; // Breakpoint where TPA is activated + } throttle; + + struct { + uint8_t rcExpo8; + uint8_t rcYawExpo8; + uint8_t rates[3]; + } stabilized; + + struct { + uint8_t rcExpo8; + uint8_t rcYawExpo8; + uint8_t rates[3]; + } manual; + } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3c21cc17497..d315e117b45 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -294,15 +294,15 @@ void annexCode(void) } else { // Compute ROLL PITCH and YAW command - rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcExpo8 : currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband); - rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcExpo8 : currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband); - rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual_rcYawExpo8 : currentControlRateProfile->rcYawExpo8, rcControlsConfig()->yaw_deadband); + rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); // Apply manual control rates if (FLIGHT_MODE(MANUAL_MODE)) { - rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual_rates[FD_ROLL] / 100L; - rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual_rates[FD_PITCH] / 100L; - rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual_rates[FD_YAW] / 100L; + rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L; + rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L; + rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L; } //Compute THROTTLE command diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9181460f4cc..3aa60a7736e 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -522,15 +522,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_RC_TUNING: sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used - sbufWriteU8(dst, currentControlRateProfile->rcExpo8); + sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); for (int i = 0 ; i < 3; i++) { - sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t + sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t } - sbufWriteU8(dst, currentControlRateProfile->dynThrPID); - sbufWriteU8(dst, currentControlRateProfile->thrMid8); - sbufWriteU8(dst, currentControlRateProfile->thrExpo8); - sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint); - sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8); + sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); + sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8); + sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8); + sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint); + sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); break; case MSP_PID: @@ -1339,23 +1339,23 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize >= 10) { sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons // need to cast away const to set controlRateProfile - ((controlRateConfig_t*)currentControlRateProfile)->rcExpo8 = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = sbufReadU8(src); for (int i = 0; i < 3; i++) { rate = sbufReadU8(src); if (i == FD_YAW) { - ((controlRateConfig_t*)currentControlRateProfile)->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); } else { - ((controlRateConfig_t*)currentControlRateProfile)->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); } } rate = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); - ((controlRateConfig_t*)currentControlRateProfile)->thrMid8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->thrExpo8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->tpa_breakpoint = sbufReadU16(src); + ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src); if (dataSize >= 11) { - ((controlRateConfig_t*)currentControlRateProfile)->rcYawExpo8 = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadU8(src); } schedulePidGainsUpdate(); diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index f7c98c3a9af..e83e4b3e596 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -208,20 +208,20 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t int newValue; switch (adjustmentFunction) { case ADJUSTMENT_RC_EXPO: - newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c - controlRateConfig->rcExpo8 = newValue; + newValue = constrain((int)controlRateConfig->stabilized.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c + controlRateConfig->stabilized.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 serial_cli.c - controlRateConfig->thrExpo8 = newValue; + newValue = constrain((int)controlRateConfig->throttle.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c + controlRateConfig->throttle.rcExpo8 = newValue; generateThrottleCurve(controlRateConfig); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_PITCH] = newValue; + newValue = constrain((int)controlRateConfig->stabilized.rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->stabilized.rates[FD_PITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { schedulePidGainsUpdate(); @@ -231,14 +231,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->rates[FD_ROLL] = newValue; + newValue = constrain((int)controlRateConfig->stabilized.rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + controlRateConfig->stabilized.rates[FD_ROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_RATE: - newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); - controlRateConfig->rates[FD_YAW] = newValue; + newValue = constrain((int)controlRateConfig->stabilized.rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + controlRateConfig->stabilized.rates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); schedulePidGainsUpdate(); break; diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 66d2b55f30f..f7516fe8529 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -38,16 +38,16 @@ int16_t lookupThrottleRCMid; // THROTTLE curve mid point void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) { - lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - const int16_t tmp = 10 * i - controlRateConfig->thrMid8; + const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8; uint8_t y = 1; if (tmp > 0) - y = 100 - controlRateConfig->thrMid8; + y = 100 - controlRateConfig->throttle.rcMid8; if (tmp < 0) - y = controlRateConfig->thrMid8; - lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; + y = controlRateConfig->throttle.rcMid8; + lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10; lookupThrottleRC[i] = motorConfig()->minthrottle + (int32_t) (motorConfig()->maxthrottle - motorConfig()->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 893e31e1c1e..c6ee0760760 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -555,61 +555,62 @@ groups: headers: ["fc/controlrate_profile.h"] value_type: CONTROL_RATE_VALUE members: - - name: rc_expo - field: rcExpo8 - min: 0 - max: 100 - - name: rc_manual_expo - field: manual_rcExpo8 + - name: thr_mid + field: throttle.rcMid8 min: 0 max: 100 - - name: rc_yaw_expo - field: rcYawExpo8 + - name: thr_expo + field: throttle.rcExpo8 min: 0 max: 100 - - name: rc_manual_yaw_expo - field: manual_rcYawExpo8 + - name: tpa_rate + field: throttle.dynPID min: 0 - max: 100 - - name: thr_mid - field: thrMid8 + max: CONTROL_RATE_CONFIG_TPA_MAX + - name: tpa_breakpoint + field: throttle.pa_breakpoint + min: PWM_RANGE_MIN + max: PWM_RANGE_MAX + - name: rc_expo + field: stabilized.rcExpo8 min: 0 max: 100 - - name: thr_expo - field: thrExpo8 + - name: rc_yaw_expo + field: stabilized.rcYawExpo8 min: 0 max: 100 # New rates are in dps/10. That means, Rate of 20 means 200dps of rotation speed on given axis. # Rate 180 (1800dps) is max. value gyro can measure reliably - name: roll_rate - field: rates[FD_ROLL] + field: stabilized.rates[FD_ROLL] min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX - name: pitch_rate - field: rates[FD_PITCH] + field: stabilized.rates[FD_PITCH] min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX - name: yaw_rate - field: rates[FD_YAW] + field: stabilized.rates[FD_YAW] min: CONTROL_RATE_CONFIG_YAW_RATE_MIN max: CONTROL_RATE_CONFIG_YAW_RATE_MAX - - name: tpa_rate - field: dynThrPID + - name: manual_rc_expo + field: manual.rcExpo8 min: 0 - max: CONTROL_RATE_CONFIG_TPA_MAX - - name: tpa_breakpoint - min: PWM_RANGE_MIN - max: PWM_RANGE_MAX + max: 100 + - name: manual_rc_yaw_expo + field: manual.rcYawExpo8 + min: 0 + max: 100 - name: manual_roll_rate - field: manual_rates[FD_ROLL] + field: manual.rates[FD_ROLL] min: 0 max: 100 - name: manual_pitch_rate - field: manual_rates[FD_PITCH] + field: manual.rates[FD_PITCH] min: 0 max: 100 - name: manual_yaw_rate - field: manual_rates[FD_YAW] + field: manual.rates[FD_YAW] min: 0 max: 100 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 9f8aac88a0e..814badf8981 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -249,7 +249,7 @@ void failsafeApplyControlInput(void) if (STATE(FIXED_WING)) { autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); - autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->rates[FD_YAW]); + autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; } else { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e36114f0737..fdac832137b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -270,10 +270,10 @@ static float calculateFixedWingTPAFactor(void) // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlRateProfile->dynThrPID != 0 && currentControlRateProfile->tpa_breakpoint > motorConfig()->minthrottle) { + if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > motorConfig()->minthrottle) { if (rcCommand[THROTTLE] > motorConfig()->minthrottle) { // Calculate TPA according to throttle - tpaFactor = 0.5f + ((float)(currentControlRateProfile->tpa_breakpoint - motorConfig()->minthrottle) / (rcCommand[THROTTLE] - motorConfig()->minthrottle) / 2.0f); + tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - motorConfig()->minthrottle) / (rcCommand[THROTTLE] - motorConfig()->minthrottle) / 2.0f); // Limit to [0.5; 2] range tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f); @@ -283,7 +283,7 @@ static float calculateFixedWingTPAFactor(void) } // Attenuate TPA curve according to configured amount - tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->dynThrPID / 100.0f); + tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->throttle.dynPID / 100.0f); } else { tpaFactor = 1.0f; @@ -297,12 +297,12 @@ static float calculateMultirotorTPAFactor(void) float tpaFactor; // TPA should be updated only when TPA is actually set - if (currentControlRateProfile->dynThrPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { + if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) { tpaFactor = 1.0f; } else if (rcCommand[THROTTLE] < motorConfig()->maxthrottle) { - tpaFactor = (100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcCommand[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (motorConfig()->maxthrottle - currentControlRateProfile->tpa_breakpoint)) / 100.0f; + tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (motorConfig()->maxthrottle - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; } else { - tpaFactor = (100 - currentControlRateProfile->dynThrPID) / 100.0f; + tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f; } return tpaFactor; @@ -387,7 +387,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); - float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->rates[axis] * 10.0f, currentControlRateProfile->rates[axis] * 10.0f); + float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); // Apply simple LPF to angleRateTarget to make response less jerky // Ideas behind this: @@ -682,15 +682,15 @@ static void pidTurnAssistant(pidState_t *pidState) imuTransformVectorEarthToBody(&targetRates); // Add in roll and pitch - pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.V.X, -currentControlRateProfile->rates[ROLL] * 10.0f, currentControlRateProfile->rates[ROLL] * 10.0f); - pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.V.Y, -currentControlRateProfile->rates[PITCH] * 10.0f, currentControlRateProfile->rates[PITCH] * 10.0f); + pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.V.X, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); + pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.V.Y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes if (STATE(FIXED_WING)) { - pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.V.Z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->rates[YAW] * 10.0f, currentControlRateProfile->rates[YAW] * 10.0f); + pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.V.Z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f); } else { - pidState[YAW].rateTarget = constrainf(targetRates.V.Z, -currentControlRateProfile->rates[YAW] * 10.0f, currentControlRateProfile->rates[YAW] * 10.0f); + pidState[YAW].rateTarget = constrainf(targetRates.V.Z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f); } } #endif @@ -713,7 +713,7 @@ void pidController(void) if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) { rateTarget = pidHeadingHold(); } else { - rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->rates[axis]); + rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]); } // Limit desired rate to something gyro can measure reliably diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index d2c5ef505b5..8417b12efe5 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -165,7 +165,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa { const timeMs_t currentTimeMs = millis(); const float absDesiredRateDps = fabsf(desiredRateDps); - float maxDesiredRate = currentControlRateProfile->rates[axis] * 10.0f; + float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; pidAutotuneState_e newState; // Use different max desired rate in ANGLE for pitch and roll diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f016715184f..1a0290d1403 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -524,7 +524,7 @@ void applyFixedWingEmergencyLandingController(void) // FIXME: Use altitude controller if available (similar to MC code) rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->rates[FD_YAW]); + rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; } diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index b41c71e75dd..b834940d7c4 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -196,13 +196,13 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_VEL_XY].I = 20; pidProfileMutable()->bank_mc.pid[PID_VEL_XY].D = 70; - ((controlRateConfig_t*)currentControlRateProfile)->rcExpo8 = 60; - ((controlRateConfig_t*)currentControlRateProfile)->rcYawExpo8 = 35; - ((controlRateConfig_t*)currentControlRateProfile)->rates[FD_ROLL] = 54; - ((controlRateConfig_t*)currentControlRateProfile)->rates[FD_PITCH] = 54; - ((controlRateConfig_t*)currentControlRateProfile)->rates[FD_YAW] = 36; - ((controlRateConfig_t*)currentControlRateProfile)->thrMid8 = 50; - ((controlRateConfig_t*)currentControlRateProfile)->thrExpo8 = 0; - ((controlRateConfig_t*)currentControlRateProfile)->dynThrPID = 10; - ((controlRateConfig_t*)currentControlRateProfile)->tpa_breakpoint = 1600; + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = 60; + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = 35; + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = 54; + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = 54; + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = 36; + ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = 50; + ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0; + ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10; + ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600; }