diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7d5aaee798..c0e6b58c2f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -755,7 +755,7 @@ static void writeIntraframe(void) * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. * Throttle lies in range [minthrottle..maxthrottle]: */ - blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue()); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -809,7 +809,7 @@ static void writeIntraframe(void) } //Motors can be below minthrottle when disarmed, but that doesn't happen much - blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue()); //Motors tend to be similar to each other so use the first motor's value as a predictor of the others const int motorCount = getMotorCount(); @@ -1618,10 +1618,10 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf)); BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name); BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); - BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); + BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", getThrottleIdleValue()); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f)); - BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorConfig()->minthrottle,motorConfig()->maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 2a3a7610d7..15916826fa 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -42,7 +42,7 @@ static const OSD_Entry menuMiscEntries[]= { OSD_LABEL_ENTRY("-- MISC --"), - OSD_SETTING_ENTRY("MIN THR", SETTING_MIN_THROTTLE), + OSD_SETTING_ENTRY("THR IDLE", SETTING_THROTTLE_IDLE), #if defined(USE_OSD) && defined(USE_ADC) OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 497e153be6..5d417f702b 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -793,9 +793,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } if (thrTiltCompStrength) { - rcCommand[THROTTLE] = constrain(motorConfig()->minthrottle - + (rcCommand[THROTTLE] - motorConfig()->minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), - motorConfig()->minthrottle, + rcCommand[THROTTLE] = constrain(getThrottleIdleValue() + + (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), + getThrottleIdleValue(), motorConfig()->maxthrottle); } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ccc9586136..75db8a94b1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -702,7 +702,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_MISC: sbufWriteU16(dst, PWM_RANGE_MIDDLE); - sbufWriteU16(dst, motorConfig()->minthrottle); + sbufWriteU16(dst, 0); // Was min_throttle sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->mincommand); @@ -732,7 +732,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_MISC: sbufWriteU16(dst, PWM_RANGE_MIDDLE); - sbufWriteU16(dst, motorConfig()->minthrottle); + sbufWriteU16(dst, 0); //Was min_throttle sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->mincommand); @@ -1712,7 +1712,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize >= 22) { sbufReadU16(src); // midrc - motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); + sbufReadU16(src); //Was min_throttle motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); @@ -1753,7 +1753,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize == 41) { sbufReadU16(src); // midrc - motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); + sbufReadU16(src); // was min_throttle motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index b9ff0b8fb5..ff0ce991f9 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -41,7 +41,7 @@ int16_t lookupThrottleRCMid; // THROTTLE curve mid point void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) { - lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRCMid = getThrottleIdleValue() + (int32_t)(motorConfig()->maxthrottle - getThrottleIdleValue()) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8; @@ -51,7 +51,7 @@ void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) if (tmp < 0) 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] + lookupThrottleRC[i] = getThrottleIdleValue() + (int32_t) (motorConfig()->maxthrottle - getThrottleIdleValue()) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ffc15a559b..ee98c5824e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -478,10 +478,6 @@ groups: type: motorConfig_t headers: ["flight/mixer.h"] members: - - name: min_throttle - field: minthrottle - min: PWM_RANGE_MIN - max: PWM_RANGE_MAX - name: max_throttle field: maxthrottle min: PWM_RANGE_MIN @@ -509,6 +505,10 @@ groups: field: throttleScale min: 0 max: 1 + - name: throttle_idle + field: throttleIdle + min: 4 + max: 30 - name: motor_poles field: motorPoleCount min: 4 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2fcf1251c0..b03da0541e 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -216,7 +216,7 @@ bool failsafeRequiresMotorStop(void) { return failsafeState.active && failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && - failsafeConfig()->failsafe_throttle < motorConfig()->minthrottle; + failsafeConfig()->failsafe_throttle < getThrottleIdleValue(); } void failsafeStartMonitoring(void) @@ -287,7 +287,7 @@ void failsafeApplyControlInput(void) break; case THROTTLE: - rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->minthrottle; + rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : getThrottleIdleValue(); break; } break; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 60c8e03f06..e8da35a01d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -61,6 +61,7 @@ static float mixerScale = 1.0f; static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static EXTENDED_FASTRAM uint8_t motorCount = 0; EXTENDED_FASTRAM int mixerThrottleCommand; +static EXTENDED_FASTRAM int throttleIdleValue = 1150; PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); @@ -96,7 +97,6 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 4); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, - .minthrottle = DEFAULT_MIN_THROTTLE, .motorPwmProtocol = DEFAULT_PWM_PROTOCOL, .motorPwmRate = DEFAULT_PWM_RATE, .maxthrottle = DEFAULT_MAX_THROTTLE, @@ -104,6 +104,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorAccelTimeMs = 0, .motorDecelTimeMs = 0, .digitalIdleOffsetValue = 450, // Same scale as in Betaflight + .throttleIdle = 15.0f, .throttleScale = 1.0f, .motorPoleCount = 14 // Most brushless motors that we use are 14 poles ); @@ -113,6 +114,11 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTO typedef void (*motorRateLimitingApplyFnPtr)(const float dT); static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn; +int getThrottleIdleValue(void) +{ + return throttleIdleValue; +} + static void computeMotorCount(void) { motorCount = 0; @@ -174,7 +180,7 @@ void applyMotorRateLimiting(const float dT) } else { // Calculate max motor step - const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle; + const uint16_t motorRange = motorConfig()->maxthrottle - throttleIdleValue; const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f); const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f); @@ -183,12 +189,12 @@ void applyMotorRateLimiting(const float dT) motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc); // Handle throttle below min_throttle (motor start/stop) - if (motorPrevious[i] < motorConfig()->minthrottle) { - if (motor[i] < motorConfig()->minthrottle) { + if (motorPrevious[i] < throttleIdleValue) { + if (motor[i] < throttleIdleValue) { motorPrevious[i] = motor[i]; } else { - motorPrevious[i] = motorConfig()->minthrottle; + motorPrevious[i] = throttleIdleValue; } } } @@ -202,6 +208,7 @@ void applyMotorRateLimiting(const float dT) void mixerInit(void) { + throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle); computeMotorCount(); loadPrimaryMotorMixer(); // in 3D mode, mixer gain has to be halved @@ -237,8 +244,8 @@ void FAST_CODE NOINLINE writeMotors(void) const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue; if (feature(FEATURE_3D)) { - if (motor[i] >= motorConfig()->minthrottle && motor[i] <= flight3DConfig()->deadband3d_low) { - motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, dshotMinThrottleOffset + DSHOT_MIN_THROTTLE); + if (motor[i] >= throttleIdleValue && motor[i] <= flight3DConfig()->deadband3d_low) { + motorValue = scaleRangef(motor[i], throttleIdleValue, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, dshotMinThrottleOffset + DSHOT_MIN_THROTTLE); motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW); } else if (motor[i] >= flight3DConfig()->deadband3d_high && motor[i] <= motorConfig()->maxthrottle) { @@ -250,11 +257,11 @@ void FAST_CODE NOINLINE writeMotors(void) } } else { - if (motor[i] < motorConfig()->minthrottle) { // motor disarmed + if (motor[i] < throttleIdleValue) { // motor disarmed motorValue = DSHOT_DISARM_COMMAND; } else { - motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE); + motorValue = scaleRangef(motor[i], throttleIdleValue, motorConfig()->maxthrottle, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE); motorValue = constrain(motorValue, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE); } } @@ -337,7 +344,7 @@ void FAST_CODE NOINLINE mixTable(const float dT) // Find min and max throttle based on condition. #ifdef USE_GLOBAL_FUNCTIONS if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) { - throttleMin = motorConfig()->minthrottle; + throttleMin = throttleIdleValue; throttleMax = motorConfig()->maxthrottle; mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax); } else @@ -347,7 +354,7 @@ void FAST_CODE NOINLINE mixTable(const float dT) if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig()->deadband3d_low; - throttleMin = motorConfig()->minthrottle; + throttleMin = throttleIdleValue; throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE]; } else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling throttleMax = motorConfig()->maxthrottle; @@ -355,14 +362,14 @@ void FAST_CODE NOINLINE mixTable(const float dT) throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive mixerThrottleCommand = throttleMax = flight3DConfig()->deadband3d_low; - throttleMin = motorConfig()->minthrottle; + throttleMin = throttleIdleValue; } else { // Deadband handling from positive to negative throttleMax = motorConfig()->maxthrottle; mixerThrottleCommand = throttleMin = flight3DConfig()->deadband3d_high; } } else { mixerThrottleCommand = rcCommand[THROTTLE]; - throttleMin = motorConfig()->minthrottle; + throttleMin = throttleIdleValue; throttleMax = motorConfig()->maxthrottle; // Throttle scaling to limit max throttle when battery is full @@ -404,12 +411,12 @@ void FAST_CODE NOINLINE mixTable(const float dT) motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) { - motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low); + motor[i] = constrain(motor[i], throttleIdleValue, flight3DConfig()->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle); } } else { - motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle); + motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle); } // Motor stop handling @@ -417,7 +424,7 @@ void FAST_CODE NOINLINE mixTable(const float dT) if (feature(FEATURE_MOTOR_STOP)) { motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand); } else { - motor[i] = motorConfig()->minthrottle; + motor[i] = throttleIdleValue; } } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 13f27dc91d..9fc9d35b75 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -83,7 +83,6 @@ PG_DECLARE(flight3DConfig_t, flight3DConfig); typedef struct motorConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) - 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) @@ -91,6 +90,7 @@ typedef struct motorConfig_s { uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] uint16_t digitalIdleOffsetValue; + float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent float throttleScale; // Scaling factor for throttle. uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry } motorConfig_t; @@ -107,6 +107,7 @@ extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; +int getThrottleIdleValue(void); uint8_t getMotorCount(void); float getMotorMixRange(void); bool mixerIsOutputSaturated(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9d5d09fa7e..73ab84b9ab 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -322,7 +322,7 @@ void pidResetTPAFilter(void) { if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f); - pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle); + pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue()); } } @@ -370,10 +370,10 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) // 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->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > motorConfig()->minthrottle) { - if (throttle > motorConfig()->minthrottle) { + if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) { + if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle - tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - motorConfig()->minthrottle) / (throttle - motorConfig()->minthrottle) / 2.0f); + tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); // Limit to [0.5; 2] range tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f); diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index fc2e7706fc..5715000714 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -79,7 +79,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) { static float estimatePitchPower(float pitch) { int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch)); altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); - const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle); + const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue()); return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ae475fedf1..f7d0891761 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -484,7 +484,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat isAutoThrottleManuallyIncreased = false; } - rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle); } #ifdef NAV_FIXED_WING_LANDING @@ -497,7 +497,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) { // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled - rcCommand[THROTTLE] = motorConfig()->minthrottle; + rcCommand[THROTTLE] = getThrottleIdleValue(); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 1ee58dbbee..ebdd30b5ce 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -138,10 +138,10 @@ static void applyFixedWingLaunchIdleLogic(void) pidResetTPAFilter(); // Throttle control logic - if (navConfig()->fw.launch_idle_throttle <= motorConfig()->minthrottle) + if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue()) { ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle + rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle } else { @@ -155,7 +155,7 @@ static void applyFixedWingLaunchIdleLogic(void) const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME); rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, - motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle); + getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); } } } @@ -200,7 +200,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) // Increase throttle gradually over `launch_motor_spinup_time` milliseconds if (navConfig()->fw.launch_motor_spinup_time > 0) { const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); - const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle); + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, 0.0f, navConfig()->fw.launch_motor_spinup_time, minIdleThrottle, navConfig()->fw.launch_throttle); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2c4bca8bb3..596b47f646 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -104,7 +104,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle; + const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle; posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); @@ -116,7 +116,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) bool adjustMulticopterAltitudeFromRCInput(void) { if (posControl.flags.isTerrainFollowEnabled) { - const float altTarget = scaleRangef(rcCommand[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude); + const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude); // In terrain follow mode we apply different logic for terrain control if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { @@ -144,7 +144,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) } else { // Scaling from minthrottle to altHoldThrottleRCZero - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); } updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); @@ -181,7 +181,7 @@ void setupMulticopterAltitudeController(void) // Make sure we are able to satisfy the deadband altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, - motorConfig()->minthrottle + rcControlsConfig()->alt_hold_deadband + 10, + getThrottleIdleValue() + rcControlsConfig()->alt_hold_deadband + 10, motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10); // Force AH controller to initialize althold integral for pending takeoff on reset @@ -249,7 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); // Save processed throttle for future use rcCommandAdjustedThrottle = rcCommand[THROTTLE]; @@ -721,12 +721,12 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); } else { /* Sensors has gone haywire, attempt to land regardless */ if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) { - rcCommand[THROTTLE] = motorConfig()->minthrottle; + rcCommand[THROTTLE] = getThrottleIdleValue(); } else { rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 595dd117d1..cc1e81203f 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -57,7 +57,6 @@ void targetConfiguration(void) if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED; motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfigMutable()->minthrottle = 1000; } pidProfileMutable()->bank_mc.pid[ROLL].P = 36; diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 36f871324a..f57dae6317 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -63,7 +63,6 @@ void targetConfiguration(void) if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED; motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfigMutable()->minthrottle = 1000; } if (hardwareRevision == AFF4_REV_1) { rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048; diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index e11cce5b2f..3fcda3d1b2 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -64,7 +64,6 @@ void targetConfiguration(void) if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfigMutable()->minthrottle = 1000; } if (hardwareRevision == AFF7_REV_1) { diff --git a/src/main/target/ASGARD32F4/config.c b/src/main/target/ASGARD32F4/config.c index 44ba43c0cd..eb6793cbff 100644 --- a/src/main/target/ASGARD32F4/config.c +++ b/src/main/target/ASGARD32F4/config.c @@ -37,6 +37,5 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; motorConfigMutable()->motorPwmRate = 4000; - motorConfigMutable()->minthrottle = 1075; motorConfigMutable()->maxthrottle = 1950; } diff --git a/src/main/target/ASGARD32F7/config.c b/src/main/target/ASGARD32F7/config.c index 44ba43c0cd..eb6793cbff 100644 --- a/src/main/target/ASGARD32F7/config.c +++ b/src/main/target/ASGARD32F7/config.c @@ -37,6 +37,5 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; motorConfigMutable()->motorPwmRate = 4000; - motorConfigMutable()->minthrottle = 1075; motorConfigMutable()->maxthrottle = 1950; } diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index 231f0dd6b1..32aa400b9d 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -82,7 +82,6 @@ void targetConfiguration(void) blackboxConfigMutable()->rate_num = 1; blackboxConfigMutable()->rate_denom = 4; - motorConfigMutable()->minthrottle = 1015; motorConfigMutable()->maxthrottle = 2000; motorConfigMutable()->mincommand = 980; motorConfigMutable()->motorPwmRate = 2000;