1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00

Drop max_throttle settings and set it hardcoded to 1850 for

rovers and boats and 2000 for multicopters and fixedwings.
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-04-09 20:53:15 +02:00
parent cea4397f7b
commit 3f9d10f155
13 changed files with 53 additions and 43 deletions

View file

@ -1844,9 +1844,9 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->craftName); 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("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", getThrottleIdleValue()); 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("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); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
#ifdef USE_ADC #ifdef USE_ADC

View file

@ -733,7 +733,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, PWM_RANGE_MIDDLE); sbufWriteU16(dst, PWM_RANGE_MIDDLE);
sbufWriteU16(dst, 0); // Was min_throttle sbufWriteU16(dst, 0); // Was min_throttle
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, getMaxThrottle());
sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); 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, PWM_RANGE_MIDDLE);
sbufWriteU16(dst, 0); //Was min_throttle sbufWriteU16(dst, 0); //Was min_throttle
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, getMaxThrottle());
sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
@ -1858,7 +1858,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src); // midrc sbufReadU16(src); // midrc
sbufReadU16(src); //Was min_throttle 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); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, 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); // midrc
sbufReadU16(src); // was min_throttle 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); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);

View file

@ -39,7 +39,7 @@ int16_t lookupThrottleRCMid; // THROTTLE curve mid point
void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
{ {
const int minThrottle = getThrottleIdleValue(); 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++) { for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8; const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
@ -49,7 +49,7 @@ void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
if (tmp < 0) if (tmp < 0)
y = controlRateConfig->throttle.rcMid8; 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] = 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) uint16_t rcLookupThrottle(uint16_t absoluteDeflection)
{ {
if (absoluteDeflection > 999) if (absoluteDeflection > 999)
return motorConfig()->maxthrottle; return getMaxThrottle();
const uint8_t lookupStep = absoluteDeflection / 100; const uint8_t lookupStep = absoluteDeflection / 100;
return lookupThrottleRC[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupThrottleRC[lookupStep + 1] - lookupThrottleRC[lookupStep]) / 100; return lookupThrottleRC[lookupStep] + (absoluteDeflection - lookupStep * 100) * (lookupThrottleRC[lookupStep + 1] - lookupThrottleRC[lookupStep]) / 100;

View file

@ -732,12 +732,6 @@ groups:
type: motorConfig_t type: motorConfig_t
headers: ["flight/mixer.h"] headers: ["flight/mixer.h"]
members: 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 - 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." 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 default_value: 1000

View file

@ -41,8 +41,8 @@ void dynamicLpfGyroTask(void) {
return; return;
} }
const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), getMaxThrottle());
const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), getMaxThrottle(), 0.0f, 1.0f);
const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo); const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq); DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);

View file

@ -57,6 +57,9 @@
#include "sensors/battery.h" #include "sensors/battery.h"
#define MAX_THROTTLE 2000
#define MAX_THROTTLE_ROVER 1850
FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS]; FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange; static float motorMixRange;
@ -83,12 +86,11 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.neutral = SETTING_3D_NEUTRAL_DEFAULT .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, PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
.motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT,
.maxthrottle = SETTING_MAX_THROTTLE_DEFAULT,
.mincommand = SETTING_MIN_COMMAND_DEFAULT, .mincommand = SETTING_MIN_COMMAND_DEFAULT,
.motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles .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) int getThrottleIdleValue(void)
{ {
if (!throttleIdleValue) { 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; return throttleIdleValue;
@ -242,11 +244,11 @@ void mixerResetDisarmedMotors(void)
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {
motorZeroCommand = reversibleMotorsConfig()->neutral; motorZeroCommand = reversibleMotorsConfig()->neutral;
throttleRangeMin = throttleDeadbandHigh; throttleRangeMin = throttleDeadbandHigh;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = getMaxThrottle();
} else { } else {
motorZeroCommand = motorConfig()->mincommand; motorZeroCommand = motorConfig()->mincommand;
throttleRangeMin = throttleIdleValue; throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = getMaxThrottle();
} }
reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
@ -357,7 +359,7 @@ static void applyTurtleModeToMotors(void) {
motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised); 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 { } else {
// Disarmed mode // Disarmed mode
@ -407,7 +409,7 @@ void FAST_CODE writeMotors(void)
throttleIdleValue, throttleIdleValue,
DSHOT_DISARM_COMMAND, DSHOT_DISARM_COMMAND,
motorConfig()->mincommand, motorConfig()->mincommand,
motorConfig()->maxthrottle, getMaxThrottle(),
DSHOT_MIN_THROTTLE, DSHOT_MIN_THROTTLE,
DSHOT_MAX_THROTTLE, DSHOT_MAX_THROTTLE,
true true
@ -424,7 +426,7 @@ void FAST_CODE writeMotors(void)
throttleRangeMin, throttleRangeMin,
throttleRangeMax, throttleRangeMax,
reversibleMotorsConfig()->deadband_high, reversibleMotorsConfig()->deadband_high,
motorConfig()->maxthrottle, getMaxThrottle(),
true true
); );
} else { } else {
@ -551,7 +553,7 @@ void FAST_CODE mixTable(void)
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) { if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) {
throttleRangeMin = throttleIdleValue; throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = getMaxThrottle();
mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
} else } else
#endif #endif
@ -562,7 +564,7 @@ void FAST_CODE mixTable(void)
* Throttle is above deadband, FORWARD direction * Throttle is above deadband, FORWARD direction
*/ */
reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = getMaxThrottle();
throttleRangeMin = throttleDeadbandHigh; throttleRangeMin = throttleDeadbandHigh;
DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
} else if (rcCommand[THROTTLE] <= throttleDeadbandLow) { } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
@ -591,7 +593,7 @@ void FAST_CODE mixTable(void)
} else { } else {
mixerThrottleCommand = rcCommand[THROTTLE]; mixerThrottleCommand = rcCommand[THROTTLE];
throttleRangeMin = throttleIdleValue; throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = getMaxThrottle();
// Throttle scaling to limit max throttle when battery is full // Throttle scaling to limit max throttle when battery is full
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
@ -630,7 +632,7 @@ void FAST_CODE mixTable(void)
motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
if (failsafeIsActive()) { if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], motorConfig()->mincommand, getMaxThrottle());
} else { } else {
motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); 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); int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX);
if (useScaled) { if (useScaled) {
thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue); thr = (thr - throttleIdleValue) * 100 / (getMaxThrottle() - throttleIdleValue);
} else { } else {
thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); 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); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
return throttle; return throttle;
} }
return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle); return constrain(throttle, throttleIdleValue, getMaxThrottle());
} }
motorStatus_e getMotorStatus(void) motorStatus_e getMotorStatus(void)
@ -725,3 +727,18 @@ bool areMotorsRunning(void)
return false; 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;
}

View file

@ -81,7 +81,6 @@ PG_DECLARE(reversibleMotorsConfig_t, reversibleMotorsConfig);
typedef struct motorConfig_s { typedef struct motorConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) // 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 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 (50-498Hz)
uint8_t motorPwmProtocol; uint8_t motorPwmProtocol;
@ -130,4 +129,6 @@ void stopMotorsNoDelay(void);
void stopPwmAllMotors(void); void stopPwmAllMotors(void);
void loadPrimaryMotorMixer(void); void loadPrimaryMotorMixer(void);
bool areMotorsRunning(void); bool areMotorsRunning(void);
uint16_t getMaxThrottle(void);

View file

@ -466,8 +466,8 @@ static float calculateMultirotorTPAFactor(void)
// TPA should be updated only when TPA is actually set // TPA should be updated only when TPA is actually set
if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) { if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) {
tpaFactor = 1.0f; tpaFactor = 1.0f;
} else if (rcCommand[THROTTLE] < motorConfig()->maxthrottle) { } else if (rcCommand[THROTTLE] < getMaxThrottle()) {
tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(motorConfig()->maxthrottle - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f;
} else { } else {
tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f; tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f;
} }

View file

@ -647,7 +647,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
} else { } else {
correctedThrottleValue = motorConfig()->maxthrottle; correctedThrottleValue = getMaxThrottle();
} }
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle); isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
} else { } else {

View file

@ -114,7 +114,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
{ {
// Calculate min and max throttle boundaries (to compensate for integral windup) // Calculate min and max throttle boundaries (to compensate for integral windup)
const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; 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); 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) bool adjustMulticopterAltitudeFromRCInput(void)
{ {
if (posControl.flags.isTerrainFollowEnabled) { 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 // In terrain follow mode we apply different logic for terrain control
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { 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 // Make sure we can satisfy max_manual_climb_rate in both up and down directions
if (rcThrottleAdjustment > 0) { if (rcThrottleAdjustment > 0) {
// Scaling from altHoldThrottleRCZero to maxthrottle // 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 { else {
// Scaling from minthrottle to altHoldThrottleRCZero // Scaling from minthrottle to altHoldThrottleRCZero
@ -190,7 +190,7 @@ void setupMulticopterAltitudeController(void)
// Make sure we are able to satisfy the deadband // Make sure we are able to satisfy the deadband
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
getThrottleIdleValue() + rcControlsConfig()->alt_hold_deadband + 10, 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 // Force AH controller to initialize althold integral for pending takeoff on reset
// Signal for that is low throttle _and_ low actual altitude // Signal for that is low throttle _and_ low actual altitude

View file

@ -127,7 +127,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[YAW] = posControl.rcAdjustment[YAW]; 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());
} }
} }
} }

View file

@ -36,5 +36,4 @@ void targetConfiguration(void)
{ {
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
motorConfigMutable()->maxthrottle = 1950;
} }

View file

@ -36,5 +36,4 @@ void targetConfiguration(void)
{ {
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
motorConfigMutable()->maxthrottle = 1950;
} }