diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4606e8a178..c8a46532fa 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1844,9 +1844,9 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->craftName); BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", getThrottleIdleValue()); - BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", getMaxThrottle()); BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f)); - BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),getMaxThrottle()); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); #ifdef USE_ADC diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cba93fb59c..88b2e35780 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -733,7 +733,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, PWM_RANGE_MIDDLE); sbufWriteU16(dst, 0); // Was min_throttle - sbufWriteU16(dst, motorConfig()->maxthrottle); + sbufWriteU16(dst, getMaxThrottle()); sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); @@ -774,7 +774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, PWM_RANGE_MIDDLE); sbufWriteU16(dst, 0); //Was min_throttle - sbufWriteU16(dst, motorConfig()->maxthrottle); + sbufWriteU16(dst, getMaxThrottle()); sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); @@ -1858,7 +1858,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); // midrc sbufReadU16(src); //Was min_throttle - motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); + sbufReadU16(src); //Was maxThrottle motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); @@ -1906,7 +1906,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); // midrc sbufReadU16(src); // was min_throttle - motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); + sbufReadU16(src); // was maxThrottle motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 47da45113b..5453083d27 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -39,7 +39,7 @@ int16_t lookupThrottleRCMid; // THROTTLE curve mid point void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) { const int minThrottle = getThrottleIdleValue(); - lookupThrottleRCMid = minThrottle + (int32_t)(motorConfig()->maxthrottle - minThrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRCMid = minThrottle + (int32_t)(getMaxThrottle() - minThrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8; @@ -49,7 +49,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] = minThrottle + (int32_t) (motorConfig()->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRC[i] = minThrottle + (int32_t) (getMaxThrottle() - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } } @@ -62,7 +62,7 @@ int16_t rcLookup(int32_t stickDeflection, uint8_t expo) uint16_t rcLookupThrottle(uint16_t absoluteDeflection) { if (absoluteDeflection > 999) - return motorConfig()->maxthrottle; + return getMaxThrottle(); const uint8_t lookupStep = absoluteDeflection / 100; return lookupThrottleRC[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupThrottleRC[lookupStep + 1] - lookupThrottleRC[lookupStep]) / 100; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index cb586eacf0..b2393250fb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -732,12 +732,6 @@ groups: type: motorConfig_t headers: ["flight/mixer.h"] members: - - name: max_throttle - description: "This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000." - default_value: 1850 - field: maxthrottle - min: PWM_RANGE_MIN - max: PWM_RANGE_MAX - name: min_command description: "This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once." default_value: 1000 diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c index 5751c84e85..0e5b4f6ef2 100644 --- a/src/main/flight/dynamic_lpf.c +++ b/src/main/flight/dynamic_lpf.c @@ -41,8 +41,8 @@ void dynamicLpfGyroTask(void) { return; } - const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); - const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); + const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle()); + const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), getMaxThrottle(), 0.0f, 1.0f); const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo); DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 834841e658..d2a073f26e 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -57,6 +57,9 @@ #include "sensors/battery.h" +#define MAX_THROTTLE 2000 +#define MAX_THROTTLE_ROVER 1850 + FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS]; FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; static float motorMixRange; @@ -83,12 +86,11 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 10); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 11); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, - .maxthrottle = SETTING_MAX_THROTTLE_DEFAULT, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles ); @@ -106,7 +108,7 @@ void pgResetFn_timerOverrides(timerOverride_t *instance) int getThrottleIdleValue(void) { if (!throttleIdleValue) { - throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle); + throttleIdleValue = motorConfig()->mincommand + (((getMaxThrottle() - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle); } return throttleIdleValue; @@ -242,11 +244,11 @@ void mixerResetDisarmedMotors(void) if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; throttleRangeMin = throttleDeadbandHigh; - throttleRangeMax = motorConfig()->maxthrottle; + throttleRangeMax = getMaxThrottle(); } else { motorZeroCommand = motorConfig()->mincommand; throttleRangeMin = throttleIdleValue; - throttleRangeMax = motorConfig()->maxthrottle; + throttleRangeMax = getMaxThrottle(); } reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; @@ -357,7 +359,7 @@ static void applyTurtleModeToMotors(void) { motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); - motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle); + motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, getMaxThrottle()); } } else { // Disarmed mode @@ -407,7 +409,7 @@ void FAST_CODE writeMotors(void) throttleIdleValue, DSHOT_DISARM_COMMAND, motorConfig()->mincommand, - motorConfig()->maxthrottle, + getMaxThrottle(), DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, true @@ -424,7 +426,7 @@ void FAST_CODE writeMotors(void) throttleRangeMin, throttleRangeMax, reversibleMotorsConfig()->deadband_high, - motorConfig()->maxthrottle, + getMaxThrottle(), true ); } else { @@ -551,7 +553,7 @@ void FAST_CODE mixTable(void) #ifdef USE_PROGRAMMING_FRAMEWORK if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) { throttleRangeMin = throttleIdleValue; - throttleRangeMax = motorConfig()->maxthrottle; + throttleRangeMax = getMaxThrottle(); mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); } else #endif @@ -562,7 +564,7 @@ void FAST_CODE mixTable(void) * Throttle is above deadband, FORWARD direction */ reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; - throttleRangeMax = motorConfig()->maxthrottle; + throttleRangeMax = getMaxThrottle(); throttleRangeMin = throttleDeadbandHigh; DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) { @@ -591,7 +593,7 @@ void FAST_CODE mixTable(void) } else { mixerThrottleCommand = rcCommand[THROTTLE]; throttleRangeMin = throttleIdleValue; - throttleRangeMax = motorConfig()->maxthrottle; + throttleRangeMax = getMaxThrottle(); // Throttle scaling to limit max throttle when battery is full #ifdef USE_PROGRAMMING_FRAMEWORK @@ -630,7 +632,7 @@ void FAST_CODE mixTable(void) motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); if (failsafeIsActive()) { - motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); + motor[i] = constrain(motor[i], motorConfig()->mincommand, getMaxThrottle()); } else { motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } @@ -652,7 +654,7 @@ int16_t getThrottlePercent(bool useScaled) int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX); if (useScaled) { - thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue); + thr = (thr - throttleIdleValue) * 100 / (getMaxThrottle() - throttleIdleValue); } else { thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } @@ -667,7 +669,7 @@ uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop) ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); return throttle; } - return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle); + return constrain(throttle, throttleIdleValue, getMaxThrottle()); } motorStatus_e getMotorStatus(void) @@ -725,3 +727,18 @@ bool areMotorsRunning(void) return false; } + +uint16_t getMaxThrottle() { + + static uint16_t throttle = 0; + + if (throttle == 0) { + if (STATE(ROVER) || STATE(BOAT)) { + throttle = MAX_THROTTLE_ROVER; + } else { + throttle = MAX_THROTTLE; + } + } + + return throttle; +} \ No newline at end of file diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 9ee6a20654..0d40fdc11e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -81,7 +81,6 @@ PG_DECLARE(reversibleMotorsConfig_t, reversibleMotorsConfig); typedef struct motorConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) - 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; @@ -130,4 +129,6 @@ void stopMotorsNoDelay(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); -bool areMotorsRunning(void); \ No newline at end of file +bool areMotorsRunning(void); + +uint16_t getMaxThrottle(void); \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3da150e39d..6f296c54b5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -466,8 +466,8 @@ static float calculateMultirotorTPAFactor(void) // TPA should be updated only when TPA is actually set 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->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(motorConfig()->maxthrottle - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; + } else if (rcCommand[THROTTLE] < getMaxThrottle()) { + tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; } else { tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f; } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e6209f460b..49fbf11ab5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -647,7 +647,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); } else { - correctedThrottleValue = motorConfig()->maxthrottle; + correctedThrottleValue = getMaxThrottle(); } isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle); } else { diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a9a6997a9f..54caccb52b 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -114,7 +114,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; - const int16_t thrCorrectionMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle; + const int16_t thrCorrectionMax = getMaxThrottle() - currentBatteryProfile->nav.mc.hover_throttle; float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0); @@ -127,7 +127,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) bool adjustMulticopterAltitudeFromRCInput(void) { if (posControl.flags.isTerrainFollowEnabled) { - const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude); + const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle(), 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) { @@ -151,7 +151,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) // Make sure we can satisfy max_manual_climb_rate in both up and down directions if (rcThrottleAdjustment > 0) { // Scaling from altHoldThrottleRCZero to maxthrottle - rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(getMaxThrottle() - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); } else { // Scaling from minthrottle to altHoldThrottleRCZero @@ -190,7 +190,7 @@ void setupMulticopterAltitudeController(void) // Make sure we are able to satisfy the deadband altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, getThrottleIdleValue() + rcControlsConfig()->alt_hold_deadband + 10, - motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10); + getMaxThrottle() - rcControlsConfig()->alt_hold_deadband - 10); // Force AH controller to initialize althold integral for pending takeoff on reset // Signal for that is low throttle _and_ low actual altitude diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c index 5f8134c6a2..8816880388 100644 --- a/src/main/navigation/navigation_rover_boat.c +++ b/src/main/navigation/navigation_rover_boat.c @@ -127,7 +127,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat rcCommand[YAW] = posControl.rcAdjustment[YAW]; } - rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, getMaxThrottle()); } } } diff --git a/src/main/target/ASGARD32F4/config.c b/src/main/target/ASGARD32F4/config.c index 9aaa2793ce..fee0992038 100644 --- a/src/main/target/ASGARD32F4/config.c +++ b/src/main/target/ASGARD32F4/config.c @@ -36,5 +36,4 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; - motorConfigMutable()->maxthrottle = 1950; } diff --git a/src/main/target/ASGARD32F7/config.c b/src/main/target/ASGARD32F7/config.c index 9aaa2793ce..fee0992038 100644 --- a/src/main/target/ASGARD32F7/config.c +++ b/src/main/target/ASGARD32F7/config.c @@ -36,5 +36,4 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; - motorConfigMutable()->maxthrottle = 1950; }