From 36857ceeee2aaa2824d78b1c790be397524e71c9 Mon Sep 17 00:00:00 2001 From: tylercorleone Date: Sat, 2 Jan 2021 21:46:57 +0100 Subject: [PATCH] Mixer smoothing (#398) * feat(mixer_smoothing): smoothing out the motor output calculation removing the harsh motorMix constraining. Also introduced thrust linearization and 'AirMode 2.0' * feat: QuickFlash's predictive AirMode, porting from another impl (#3) * min/max fix, thanks to borisbstyle * predictiveAirMode activation logic refactor * applyAirMode become applyMixerClipAdjustment * refactoring and simplification * rolling back some useless changes * cleared out thrust linearization formula and fixed the linear throttle (there was a division by 100 too much) * removed unnecessary sign management * removed unnecessary change * fix * fixes, removed throttle linearization for the moment * moved PID scaling * removed DEBUG_WRONG_PIDSUM_SIGN * thrust linearization formula changed (actual matematical inverse) and reintroduced thruttle linearization * removed duplicated code * mixer_impl * mixing yaw separately * fixes and temporarily put mixerImpl on OSD * mix & roll/pitch mix rate * norm fix * wip * fixes * final, maybe... * cleanup * unlinear throttle fix * removing avg controller's caused thrust/motor * backup * two pass mixer. Version 1.0.0 * two level thrust linearization * code cleaning * TL from idle level and TPA disabled when TL is enabled * fix SPA and motorOutputIdleLevel * changed desmos link for TL graphs * fix: applying AirMode level (so AirMode OFF) given throttleMotor, so that with boht linear_throttle ON and OFF the transition is the same * removed mixer impl from OSD and another code cleaning * mixerInitProfile called into pidInitConfig * rewording * fix: fixed thrust-linearization disabling. Code cleaning * 65 default value for linear_thrust_low_output * little code simplification * 3d mode fix and cleanup Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Co-authored-by: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com> --- src/main/blackbox/blackbox.c | 5 + src/main/cms/cms_menu_imu.c | 28 ++- src/main/common/maths.h | 1 + src/main/fc/config.c | 1 + src/main/fc/controlrate_profile.c | 2 - src/main/fc/controlrate_profile.h | 2 - src/main/flight/mixer.c | 297 +++++++++++++++++++++--------- src/main/flight/mixer.h | 3 +- src/main/flight/pid.c | 50 ++--- src/main/flight/pid.h | 17 +- src/main/interface/msp.c | 9 +- src/main/interface/settings.c | 18 +- src/main/interface/settings.h | 1 + src/main/sensors/battery.c | 38 ++-- src/main/sensors/battery.h | 2 +- src/test/unit/pid_unittest.cc | 12 +- 16 files changed, 329 insertions(+), 157 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 88e723c7d1..1628a52d37 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1204,6 +1204,11 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw); BLACKBOX_PRINT_HEADER_LINE("iterm_rotation", "%d", currentPidProfile->iterm_rotation); BLACKBOX_PRINT_HEADER_LINE("throttle_boost", "%d", currentPidProfile->throttle_boost); + BLACKBOX_PRINT_HEADER_LINE("linear_thrust_low_output", "%d", currentPidProfile->linear_thrust_low_output); + BLACKBOX_PRINT_HEADER_LINE("linear_thrust_high_output", "%d", currentPidProfile->linear_thrust_high_output); + BLACKBOX_PRINT_HEADER_LINE("linear_throttle", "%d", currentPidProfile->linear_throttle); + BLACKBOX_PRINT_HEADER_LINE("mixer_impl", "%d", currentPidProfile->mixer_impl); + BLACKBOX_PRINT_HEADER_LINE("mixer_laziness", "%d", currentPidProfile->mixer_laziness); // End of EmuFlight controller parameters BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 0959c5b51e..475e0552af 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -76,7 +76,11 @@ static uint16_t dtermBoost; static uint8_t dtermBoostLimit; static uint8_t tempPid[3][3]; static uint8_t tempPidWc[3]; - +static uint8_t linear_thrust_low_output; +static uint8_t linear_thrust_high_output; +static uint8_t linear_throttle; +static mixerImplType_e mixer_impl; +static uint8_t mixer_laziness; static uint8_t tmpRateProfileIndex; static uint8_t rateProfileIndex; @@ -91,6 +95,10 @@ static const char * const cms_throttleVbatCompTypeLabels[] = { "OFF", "BOOST", "LIMIT", "BOTH" }; +static const char * const cms_mixerImplTypeLabels[] = { + "LEGACY", "SMOOTH", "2PASS" +}; + static long cmsx_menuImu_onEnter(void) { pidProfileIndex = getCurrentPidProfileIndex(); tmpPidProfileIndex = pidProfileIndex + 1; @@ -138,6 +146,11 @@ static long cmsx_PidAdvancedRead(void) { itermRelaxThreshold = pidProfile->iterm_relax_threshold; itermRelaxThresholdYaw = pidProfile->iterm_relax_threshold_yaw; itermWindup = pidProfile->itermWindupPointPercent; + linear_thrust_low_output = pidProfile->linear_thrust_low_output; + linear_thrust_high_output = pidProfile->linear_thrust_high_output; + linear_throttle = pidProfile->linear_throttle; + mixer_impl = pidProfile->mixer_impl; + mixer_laziness = pidProfile->mixer_laziness; return 0; } @@ -164,6 +177,11 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self) { pidProfile->iterm_relax_threshold = itermRelaxThreshold; pidProfile->iterm_relax_threshold_yaw = itermRelaxThresholdYaw; pidProfile->itermWindupPointPercent = itermWindup; + pidProfile->linear_thrust_low_output = linear_thrust_low_output; + pidProfile->linear_thrust_high_output = linear_thrust_high_output; + pidProfile->linear_throttle = linear_throttle; + pidProfile->mixer_impl = mixer_impl; + pidProfile->mixer_laziness = mixer_laziness; pidInitConfig(currentPidProfile); return 0; } @@ -189,6 +207,12 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = { { "I RELAX THRESH YAW", OME_UINT8, NULL, &(OSD_UINT8_t){ &itermRelaxThresholdYaw, 0, 100, 1 }, 0 }, { "I WINDUP", OME_UINT8, NULL, &(OSD_UINT8_t){ &itermWindup, 0, 100, 1 }, 0 }, + { "LINEAR THRUST LOW", OME_UINT8, NULL, &(OSD_UINT8_t) { &linear_thrust_low_output, 0, 100, 1}, 0 }, + { "LINEAR THRUST HIGH", OME_UINT8, NULL, &(OSD_UINT8_t) { &linear_thrust_high_output, 0, 100, 1}, 0 }, + { "LINEAR THROTTLE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &linear_throttle, 1, cms_offOnLabels }, 0 }, + { "MIXER IMPL", OME_TAB, NULL, &(OSD_TAB_t) { &mixer_impl, MIXER_IMPL_COUNT - 1, cms_mixerImplTypeLabels }, 0 }, + { "MIXER LAZINESS", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &mixer_laziness, 1, cms_offOnLabels }, 0 }, + { "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0}, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -316,8 +340,6 @@ static OSD_Entry cmsx_menuRateProfileEntries[] = { { "VBAT COMP TYPE", OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.vbat_comp_type, VBAT_COMP_TYPE_COUNT - 1, cms_throttleVbatCompTypeLabels}, 0 }, { "VBAT COMP REF", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.vbat_comp_ref, VBAT_CELL_VOTAGE_RANGE_MIN, VBAT_CELL_VOTAGE_RANGE_MAX, 1}, 0 }, - { "VBAT COMP THR %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.vbat_comp_throttle_level, 0, 100, 1}, 0 }, - { "VBAT COMP PID %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.vbat_comp_pid_level, 0, 100, 1}, 0 }, { "THROTTLE LIMIT %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1}, 0 }, { "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0}, diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 8f1acc198b..2bace86d18 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -58,6 +58,7 @@ #define ABS(x) \ __extension__ ({ __typeof__ (x) _x = (x); \ _x > 0 ? _x : -_x; }) +#define SCALE_UNITARY_RANGE(x, from, to) ((1.0f - (x)) * (from) + (x) * (to)) #define Q12 (1 << 12) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 37ec601c81..5ccab8f8b5 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -576,6 +576,7 @@ void changePidProfile(uint8_t pidProfileIndex) { loadPidProfile(); pidInit(currentPidProfile); initEscEndpoints(); + mixerInitProfile(); } beeperConfirmationBeeps(pidProfileIndex + 1); } diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index f57203318c..838d70dd1f 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -64,8 +64,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) { .throttle_limit_percent = 100, .vbat_comp_type = VBAT_COMP_TYPE_OFF, .vbat_comp_ref = 37, - .vbat_comp_throttle_level = 75, - .vbat_comp_pid_level = 75, .addRollToYawRc = 0, .addYawToRollRc = 0, .rollPitchMagExpo = 0, diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 78d39d407a..a15e4d39ab 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -75,8 +75,6 @@ typedef struct controlRateConfig_s { uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit uint8_t vbat_comp_type; // Sets the type of battery compensation: off, boost, limit or both uint8_t vbat_comp_ref; // Sets the voltage reference to calculate the battery compensation - uint8_t vbat_comp_throttle_level; // Sets the level of throttle battery compensation - uint8_t vbat_comp_pid_level; // Sets the level of PID battery compensation } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8c0bc06d3b..ef7431fe8f 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -120,10 +120,10 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR #define PWM_RANGE_MID 1500 static FAST_RAM_ZERO_INIT uint8_t motorCount; -static FAST_RAM_ZERO_INIT float motorMixRange; +static FAST_RAM_ZERO_INIT float controllerMixRange; float FAST_RAM_ZERO_INIT motor[MAX_SUPPORTED_MOTORS]; -//float FAST_RAM_ZERO_INIT previousMotor[MAX_SUPPORTED_MOTORS]; + float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; @@ -131,6 +131,14 @@ static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static FAST_RAM_ZERO_INIT int throttleAngleCorrection; +FAST_RAM_ZERO_INIT bool linearThrustEnabled; +static FAST_RAM_ZERO_INIT float linearThrustLowOutput; +static FAST_RAM_ZERO_INIT float linearThrustHighOutput; +static FAST_RAM_ZERO_INIT FAST_RAM_ZERO_INIT float linearThrustPIDScaler; // used to avoid/limit PID tuning when enabling thrust linearization +static FAST_RAM_ZERO_INIT float linearThrustYawPIDScaler; // 2PASS mixer doesn't apply TL to yaw so it don't needs to compensate for that + +static FAST_RAM_ZERO_INIT mixerImplType_e mixerImpl; +static FAST_RAM_ZERO_INIT bool mixerLaziness; static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -325,12 +333,18 @@ FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow; static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; static FAST_RAM_ZERO_INIT float rcCommandThrottleRange; +static void twoPassMix(float *motorMix, const float *yawMix, const float *rollPitchMix, float yawMixMin, float yawMixMax, + float rollPitchMixMin, float rollPitchMixMax); +static void mixThingsUp(float scaledAxisPidRoll, float scaledAxisPidPitch, float scaledAxisPidYaw, float *motorMix); +static float thrustToMotor(float thrust, bool fromIdleLevelOffset); +static float motorToThrust(float motor, bool fromIdleLevelOffset); + uint8_t getMotorCount(void) { return motorCount; } -float getMotorMixRange(void) { - return motorMixRange; +float getControllerMixRange(void) { + return controllerMixRange; } bool areMotorsRunning(void) { @@ -360,7 +374,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate) { if (axis == FD_YAW && mixerIsTricopter()) { return mixerTricopterIsServoSaturated(errorRate); } - return motorMixRange >= 1.0f; + return controllerMixRange >= 1.0f; } // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator @@ -418,6 +432,7 @@ void mixerInit(mixerMode_e mixerMode) { if (mixerIsTricopter()) { mixerTricopterInit(); } + mixerInitProfile(); } #ifndef USE_QUAD_MIXER_ONLY @@ -512,7 +527,9 @@ static FAST_RAM_ZERO_INIT float motorOutputMin; static FAST_RAM_ZERO_INIT float motorRangeMin; static FAST_RAM_ZERO_INIT float motorRangeMax; static FAST_RAM_ZERO_INIT float motorOutputRange; -static FAST_RAM_ZERO_INIT int8_t motorOutputMixSign; +static FAST_RAM_ZERO_INIT float motorOutputIdleLevel; // e.g. 5% idle throttle -> 0.05 +static FAST_RAM_ZERO_INIT float motorThrustIdleLevel; // corresponding thrust level of motorOutputIdleLevel +static FAST_RAM_ZERO_INIT int8_t controllerMix3DModeSign; // 1 -> normal, -1 -> reversed static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions @@ -548,10 +565,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { motorOutputMin = deadbandMotor3dLow; motorOutputRange = motorOutputLow - deadbandMotor3dLow; } - if (motorOutputMixSign != -1) { + if (controllerMix3DModeSign != -1) { reversalTimeUs = currentTimeUs; } - motorOutputMixSign = -1; + controllerMix3DModeSign = -1; rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; currentThrottleInputRange = rcCommandThrottleRange3dLow; @@ -561,10 +578,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; - if (motorOutputMixSign != 1) { + if (controllerMix3DModeSign != 1) { reversalTimeUs = currentTimeUs; } - motorOutputMixSign = 1; + controllerMix3DModeSign = 1; rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh; @@ -581,10 +598,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { motorOutputMin = deadbandMotor3dLow; motorOutputRange = motorOutputLow - deadbandMotor3dLow; } - if (motorOutputMixSign != -1) { + if (controllerMix3DModeSign != -1) { reversalTimeUs = currentTimeUs; } - motorOutputMixSign = -1; + controllerMix3DModeSign = -1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dLow; } else { @@ -593,10 +610,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { motorRangeMax = motorOutputHigh; motorOutputMin = deadbandMotor3dHigh; motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; - if (motorOutputMixSign != 1) { + if (controllerMix3DModeSign != 1) { reversalTimeUs = currentTimeUs; } - motorOutputMixSign = 1; + controllerMix3DModeSign = 1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dHigh; } @@ -612,14 +629,26 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { motorOutputMin = motorOutputLow; motorOutputRange = motorOutputHigh - motorOutputLow; if (getBoxIdState(BOXUSER4)) { - motorOutputMixSign = -1; + controllerMix3DModeSign = -1; } else { - motorOutputMixSign = 1; + controllerMix3DModeSign = 1; } } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); } +void mixerInitProfile(void) { + linearThrustEnabled = currentPidProfile->linear_thrust_low_output && currentPidProfile->linear_thrust_high_output; // both have to be != 0 or a division by 0 could happen + mixerImpl = currentPidProfile->mixer_impl; + mixerLaziness = currentPidProfile->mixer_laziness && linearThrustEnabled; // laziness requires linear thrust + linearThrustLowOutput = 0.01f * currentPidProfile->linear_thrust_low_output; + linearThrustHighOutput = 0.01f * currentPidProfile->linear_thrust_high_output; + linearThrustPIDScaler = motorToThrust(0.5f, false) / 0.5f; // PID settings retro-compatibility: non-linear thrust / linear thrust + linearThrustYawPIDScaler = mixerImpl == MIXER_IMPL_2PASS ? 1.0f : linearThrustPIDScaler; + motorOutputIdleLevel = ABS((motorOutputLow - disarmMotorOutput) / (motorOutputHigh - disarmMotorOutput)); + motorThrustIdleLevel = motorToThrust(motorOutputIdleLevel, false); +} + #define CRASH_FLIP_DEADBAND 20 #define CRASH_FLIP_STICK_MINF 0.15f @@ -683,11 +712,10 @@ static void applyFlipOverAfterCrashModeToMotors(void) { } } -static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { - // Now add in the desired throttle, but keep in a range that doesn't clip adjusted - // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. +static void applyMixToMotors(const float motorMix[MAX_SUPPORTED_MOTORS]) { + float vbatCompFactor = calculateBatteryCompensationFactor(); for (int i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); + float motorOutput = motorOutputMin + constrainf(motorMix[i] * vbatCompFactor, 0.0f, 1.0f) * motorOutputRange; if (mixerIsTricopter()) { motorOutput += mixerTricopterMotorCorrection(i); } @@ -695,9 +723,9 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { if (isMotorProtocolDshot()) { motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } - motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); + motorOutput = constrainf(motorOutput, disarmMotorOutput, motorRangeMax); } else { - motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); + motorOutput = constrainf(motorOutput, motorRangeMin, motorRangeMax); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive() @@ -708,37 +736,6 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { } motor[i] = motorOutput; } -// float difference; -// float looptimeAccounter; -// looptimeAccounter = gyro.targetLooptime * pidConfig()->pid_process_denom; -// for (int motorNum = 0; motorNum < motorCount; motorNum++) -// { -// difference = fabsf(motor[motorNum] - previousMotor[motorNum]); -// if (difference <= (looptimeAccounter * motorOutputRange * 0.00002f)) -// { -// motor[motorNum] = previousMotor[motorNum]; -// } -// else -// { -// if (difference > (looptimeAccounter * motorOutputRange * 0.00040f)) -// { -// if (motor[motorNum] > previousMotor[motorNum]) -// { -// motor[motorNum] = previousMotor[motorNum] + (looptimeAccounter * motorOutputRange * 0.00040f); /* increase by max 5% every ms */ -// previousMotor[motorNum] = motor[motorNum]; -// } -// else -// { -// motor[motorNum] = previousMotor[motorNum] - (looptimeAccounter * motorOutputRange * 0.00040f); /* decrease by max 5% every ms */ -// previousMotor[motorNum] = motor[motorNum]; -// } -// } -// else -// { -// previousMotor[motorNum] = motor[motorNum]; -// } -// } -// } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (int i = 0; i < motorCount; i++) { @@ -747,12 +744,6 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { } } -float applyThrottleVbatCompensation(float throttle) { - float vbatCompensation = calculateVbatCompensation(currentControlRateProfile->vbat_comp_type, currentControlRateProfile->vbat_comp_ref); - float throttleVbatCompensation = scaleRangef(currentControlRateProfile->vbat_comp_throttle_level, 0.0f, 100.0f, 1.0f, vbatCompensation); - return constrainf(throttle * throttleVbatCompensation, 0.0f, 1.0f); -} - float applyThrottleLimit(float throttle) { if (currentControlRateProfile->throttle_limit_percent < 100) { const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f; @@ -766,6 +757,43 @@ float applyThrottleLimit(float throttle) { return throttle; } +void mixWithThrottleLegacy(float *motorMix, float *controllerMix, float controllerMixMin, float controllerMixMax) { + float throttleThrust = currentPidProfile->linear_throttle ? throttle : motorToThrust(throttle, true); + float normFactor = 1 / (controllerMixRange > 1.0f && hardwareMotorType != MOTOR_BRUSHED ? controllerMixRange : 1.0f); + + if (mixerImpl == MIXER_IMPL_LEGACY) { + // legacy clipping handling + if (normFactor < 1.0f) { + for (int i = 0; i < motorCount; i++) { + controllerMix[i] *= normFactor; + } + // Get the maximum correction by setting offset to center when airmode enabled + if (isAirmodeActive()) { + throttleThrust = 0.5f; + } + } else { + if (isAirmodeActive() || throttleThrust > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle + throttleThrust = constrainf(throttleThrust, -controllerMixMin, 1.0f - controllerMixMax); + } + } + } else { + float throttleMotor = currentPidProfile->linear_throttle ? thrustToMotor(throttle, true) : throttle; // used to make authority grow faster + float authority = isAirmodeActive() ? 1.0f : SCALE_UNITARY_RANGE(throttleMotor, 0.5f, 1.0f); + normFactor *= authority; + } + + for (int i = 0; i < motorCount; i++) { + if (mixerImpl == MIXER_IMPL_SMOOTH) { + // clipping handling + float offset = mixerLaziness ? (ABS(controllerMix[i]) * SCALE_UNITARY_RANGE(throttleThrust, 1, -1)) + : SCALE_UNITARY_RANGE(throttleThrust, -controllerMixMin, -controllerMixMax); + controllerMix[i] = (controllerMix[i] + offset) * normFactor; + } + float thrustMix = controllerMix[i] + throttleThrust * currentMixer[i].throttle; + motorMix[i] = thrustToMotor(thrustMix, true); + } +} + FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) { if (isFlipOverAfterCrashMode()) { applyFlipOverAfterCrashModeToMotors(); @@ -773,11 +801,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) { } // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(currentTimeUs); + // Calculate and Limit the PIDsum const float scaledAxisPidRoll = - constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; + constrainf(pidData[FD_ROLL].Sum * linearThrustPIDScaler, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; const float scaledAxisPidPitch = - constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; + constrainf(pidData[FD_PITCH].Sum * linearThrustPIDScaler, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw; #ifdef USE_YAW_SPIN_RECOVERY const bool yawSpinDetected = gyroYawSpinDetected(); @@ -785,32 +814,17 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) { yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority } #endif // USE_YAW_SPIN_RECOVERY - float scaledAxisPidYaw = constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; + float scaledAxisPidYaw = constrainf(pidData[FD_YAW].Sum * linearThrustYawPIDScaler, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; + if (!mixerConfig()->yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; } - if (currentControlRateProfile->vbat_comp_type != VBAT_COMP_TYPE_OFF) { - throttle = applyThrottleVbatCompensation(throttle); - } + // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) { throttle = applyThrottleLimit(throttle); } - // Find roll/pitch/yaw desired output - float motorMix[MAX_SUPPORTED_MOTORS]; - float motorMixMax = 0, motorMixMin = 0; - for (int i = 0; i < motorCount; i++) { - float mix = - scaledAxisPidRoll * currentMixer[i].roll + - scaledAxisPidPitch * currentMixer[i].pitch + - scaledAxisPidYaw * currentMixer[i].yaw; - if (mix > motorMixMax) { - motorMixMax = mix; - } else if (mix < motorMixMin) { - motorMixMin = mix; - } - motorMix[i] = mix; - } + #if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle); @@ -825,22 +839,57 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) { } #endif loggingThrottle = throttle; - motorMixRange = motorMixMax - motorMixMin; - if (motorMixRange > 1.0f && (hardwareMotorType != MOTOR_BRUSHED)) { - for (int i = 0; i < motorCount; i++) { - motorMix[i] /= motorMixRange; + + float motorMix[MAX_SUPPORTED_MOTORS]; + + // mix controller output with throttle + mixThingsUp(scaledAxisPidRoll, scaledAxisPidPitch, scaledAxisPidYaw, motorMix); + + // Apply the mix to motor endpoints + applyMixToMotors(motorMix); +} + +void mixThingsUp(const float scaledAxisPidRoll, const float scaledAxisPidPitch, const float scaledAxisPidYaw, float *motorMix) { + float yawMix[MAX_SUPPORTED_MOTORS]; + float rollPitchMix[MAX_SUPPORTED_MOTORS]; + float controllerMix[MAX_SUPPORTED_MOTORS]; + float yawMixMin = 0, yawMixMax = 0; + float rollPitchMixMin = 0, rollPitchMixMax = 0; + float controllerMixMin = 0, controllerMixMax = 0; + + for (int i = 0; i < motorCount; i++) { + float yawMixVal = controllerMix3DModeSign * scaledAxisPidYaw * currentMixer[i].yaw; + if (yawMixVal > yawMixMax) { + yawMixMax = yawMixVal; + } else if (yawMixVal < yawMixMin) { + yawMixMin = yawMixVal; } - // Get the maximum correction by setting offset to center when airmode enabled - if (isAirmodeActive()) { - throttle = 0.5f; + yawMix[i] = yawMixVal; + + float rollPitchMixVal = scaledAxisPidRoll * currentMixer[i].roll + scaledAxisPidPitch * currentMixer[i].pitch; + if (rollPitchMixVal > rollPitchMixMax) { + rollPitchMixMax = rollPitchMixVal; + } else if (rollPitchMixVal < rollPitchMixMin) { + rollPitchMixMin = rollPitchMixVal; } - } else { - if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle - throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax); + rollPitchMix[i] = rollPitchMixVal; + + float controllerMixVal = controllerMix3DModeSign * (rollPitchMixVal + yawMixVal); + if (controllerMixVal > controllerMixMax) { + controllerMixMax = controllerMixVal; + } else if (controllerMixVal < controllerMixMin) { + controllerMixMin = controllerMixVal; } + controllerMix[i] = controllerMixVal; + } + + controllerMixRange = controllerMixMax - controllerMixMin; // measures how much the controller is trying to compensate + + if (mixerImpl == MIXER_IMPL_2PASS) { + twoPassMix(motorMix, yawMix, rollPitchMix, yawMixMin, yawMixMax, rollPitchMixMin, rollPitchMixMax); + } else { + mixWithThrottleLegacy(motorMix, controllerMix, controllerMixMin, controllerMixMax); } - // Apply the mix to motor endpoints - applyMixToMotors(motorMix); } float convertExternalToMotor(uint16_t externalValue) { @@ -903,3 +952,67 @@ void mixerSetThrottleAngleCorrection(int correctionValue) { float mixerGetLoggingThrottle(void) { return loggingThrottle; } + +float thrustToMotor(float thrust, bool fromIdleLevelOffset) { + if (!linearThrustEnabled) { + return thrust; + } + + thrust = constrainf(thrust, 0.0f, 1.0f); + + if (fromIdleLevelOffset) { + // simply applying some shifts to the graph, for more info see https://www.desmos.com/calculator/lgtopxo5mt + float x = thrustToMotor(thrust * (1.0f - motorThrustIdleLevel) + motorThrustIdleLevel , false); + return (x - motorOutputIdleLevel) / (1.0f - motorOutputIdleLevel); + } + + float compLevel = SCALE_UNITARY_RANGE(thrust, linearThrustLowOutput, linearThrustHighOutput); + return (compLevel - 1 + sqrtf(sq(1.0f - compLevel) + 4.0f * compLevel * thrust)) / (2.0f * compLevel); +} + +float motorToThrust(float motor, bool fromIdleLevelOffset) { + if (!linearThrustEnabled) { + return motor; + } + + motor = constrainf(motor, 0.0f, 1.0f); + + if (fromIdleLevelOffset) { + // simply applying some shifts to the graph, for more info see https://www.desmos.com/calculator/lgtopxo5mt + float x = motorToThrust(motor * (1.0f - motorOutputIdleLevel) + motorOutputIdleLevel , false); + return (x - motorThrustIdleLevel) / (1.0f - motorThrustIdleLevel); + } + + float compLevel = SCALE_UNITARY_RANGE(motor, linearThrustLowOutput, linearThrustHighOutput); + return (1.0f - compLevel) * motor + compLevel * sq(motor); +} + +static void twoPassMix(float *motorMix, const float *yawMix, const float *rollPitchMix, float yawMixMin, float yawMixMax, + float rollPitchMixMin, float rollPitchMixMax) { + + float throttleMotor = currentPidProfile->linear_throttle ? thrustToMotor(throttle, true) : throttle; + float authority = isAirmodeActive() ? 1.0f : SCALE_UNITARY_RANGE(throttleMotor, 0.5f, 1.0f); + + float controllerMixNormFactor = authority / MAX(controllerMixRange, 1.0f); + + // filling up motorMix with throttle, yaw and roll/pitch + for (int i = 0; i < motorCount; i++) { + motorMix[i] = throttleMotor; // motorMix have to contain output-proportional values + + // clipping handling + float yawOffset = mixerLaziness ? (ABS(yawMix[i]) * SCALE_UNITARY_RANGE(throttleMotor, 1, -1)) + : SCALE_UNITARY_RANGE(throttleMotor, -yawMixMin, -yawMixMax); + + motorMix[i] += (yawMix[i] + yawOffset) * controllerMixNormFactor; // yaw is an output-proportional value (RPM-proportional, actually) + + float motorMixThrust = motorToThrust(motorMix[i], true); // convert into thrust value + + // clipping handling + float rollPitchOffset = mixerLaziness ? (ABS(rollPitchMix[i]) * SCALE_UNITARY_RANGE(motorMixThrust, 1, -1)) + : SCALE_UNITARY_RANGE(motorMixThrust, -rollPitchMixMin, -rollPitchMixMax); + + motorMixThrust += (rollPitchOffset + rollPitchMix[i]) * controllerMixNormFactor; // roll and pitch are thrust-proportional values + + motorMix[i] = thrustToMotor(motorMixThrust, true); // translating back into motor value + } +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 625b56d89b..cb0669f550 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -115,13 +115,14 @@ extern float motorOutputHigh, motorOutputLow; struct rxConfig_s; uint8_t getMotorCount(void); -float getMotorMixRange(void); +float getControllerMixRange(void); bool areMotorsRunning(void); bool mixerIsOutputSaturated(int axis, float errorRate); void mixerLoadMix(int index, motorMixer_t *customMixers); void initEscEndpoints(void); void mixerInit(mixerMode_e mixerMode); +void mixerInitProfile(void); void mixerConfigureOutput(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 26c43a20ce..04121af15f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -75,6 +75,8 @@ static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false; static FAST_RAM_ZERO_INIT float dT; static FAST_RAM_ZERO_INIT float pidFrequency; +extern struct pidProfile_s *currentPidProfile; +extern bool linearThrustEnabled; PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); @@ -175,6 +177,11 @@ void resetPidProfile(pidProfile_t *pidProfile) { .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY, .horizon_tilt_effect = 80, .horizonTransition = 0, + .linear_thrust_low_output = 65, + .linear_thrust_high_output = 0, + .linear_throttle = false, + .mixer_impl = MIXER_IMPL_LEGACY, + .mixer_laziness = false, .horizonStrength = 15, ); } @@ -393,6 +400,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { itermRelaxCutoffYaw = pidProfile->iterm_relax_cutoff_yaw; #endif iDecay = (float)pidProfile->i_decay; + mixerInitProfile(); } void pidInit(const pidProfile_t *pidProfile) { @@ -503,7 +511,7 @@ static void handleCrashRecovery( temporaryIterm[axis] = 0.0f; if ( cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs || - (getMotorMixRange() < 1.0f && + (getControllerMixRange() < 1.0f && ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate && ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate && ABS(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate @@ -530,7 +538,7 @@ static void detectAndSetCrashRecovery( // no point in trying to recover if the crash is so severe that the gyro overflows if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) { if (ARMING_FLAG(ARMED)) { - if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold && ABS(getSetpointRate(axis)) < crashSetpointThreshold) { + if (getControllerMixRange() >= 1.0f && !inCrashRecoveryMode && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold && ABS(getSetpointRate(axis)) < crashSetpointThreshold) { inCrashRecoveryMode = true; crashDetectedAtUs = currentTimeUs; } @@ -581,26 +589,13 @@ static FAST_RAM_ZERO_INIT float previousdDelta[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float kdRingBuffer[XYZ_AXIS_COUNT][KD_RING_BUFFER_SIZE]; static FAST_RAM_ZERO_INIT float kdRingBufferSum[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT uint8_t kdRingBufferPoint[XYZ_AXIS_COUNT]; -static FAST_RAM_ZERO_INIT float setPointPAttenuation[XYZ_AXIS_COUNT]; -static FAST_RAM_ZERO_INIT float setPointIAttenuation[XYZ_AXIS_COUNT]; -static FAST_RAM_ZERO_INIT float setPointDAttenuation[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs; void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - // calculate spa - // SPA boost if SPA > 100 SPA cut if SPA < 100 - setPointPAttenuation[axis] = 1 + (getRcDeflectionAbs(axis) * (setPointPTransition[axis] - 1)); - setPointIAttenuation[axis] = 1 + (getRcDeflectionAbs(axis) * (setPointITransition[axis] - 1)); - setPointDAttenuation[axis] = 1 + (getRcDeflectionAbs(axis) * (setPointDTransition[axis] - 1)); - } - //vbat pid compensation on just the p term :) thanks NFE - float vbatCompensationFactor = calculateVbatCompensation(currentControlRateProfile->vbat_comp_type, currentControlRateProfile->vbat_comp_ref); - vbatCompensationFactor = scaleRangef(currentControlRateProfile->vbat_comp_pid_level, 0.0f, 100.0f, 1.0f, vbatCompensationFactor); // gradually scale back integration when above windup point float dynCi = dT; if (ITermWindupPointInv != 0.0f) { - dynCi *= constrainf((1.0f - getMotorMixRange()) * ITermWindupPointInv, 0.0f, 1.0f); + dynCi *= constrainf((1.0f - getControllerMixRange()) * ITermWindupPointInv, 0.0f, 1.0f); } float errorRate; // ----------PID controller---------- @@ -674,7 +669,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } #endif // USE_ITERM_RELAX // -----calculate P component - pidData[axis].P = (pidCoefficient[axis].Kp * (boostedErrorRate + errorRate)) * vbatCompensationFactor; + pidData[axis].P = (pidCoefficient[axis].Kp * (boostedErrorRate + errorRate)); // -----calculate I component //float iterm = constrainf(pidData[axis].I + (pidCoefficient[axis].Ki * errorRate) * dynCi, -itermLimit, itermLimit); float iDecayMultiplier = iDecay; @@ -758,11 +753,22 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[axis].D = 0; pidData[axis].Sum = 0; } - // calculating the PID sum and TPA and SPA - // multiply these things to the pidData so that logs shows the pid data correctly - pidData[axis].P = pidData[axis].P * getThrottlePAttenuation() * setPointPAttenuation[axis]; - pidData[axis].I = temporaryIterm[axis] * getThrottleIAttenuation() * setPointIAttenuation[axis]; // you can't use pidData[axis].I to calculate iterm or with tpa you get issues - pidData[axis].D = pidData[axis].D * getThrottleDAttenuation() * setPointDAttenuation[axis]; + + // applying SetPointAttenuation + // SPA boost if SPA > 100 SPA cut if SPA < 100 + float setPointPAttenuation = 1 + (getRcDeflectionAbs(axis) * (setPointPTransition[axis] - 1)); + float setPointIAttenuation = 1 + (getRcDeflectionAbs(axis) * (setPointITransition[axis] - 1)); + float setPointDAttenuation = 1 + (getRcDeflectionAbs(axis) * (setPointDTransition[axis] - 1)); + pidData[axis].P *= setPointPAttenuation; + pidData[axis].I = temporaryIterm[axis] * setPointIAttenuation; // you can't use pidData[axis].I to calculate iterm or with tpa you get issues + pidData[axis].D *= setPointDAttenuation; + + if (!linearThrustEnabled) { // thrust linearization already solves high throttle PID problems + pidData[axis].P *= getThrottlePAttenuation(); + pidData[axis].I *= getThrottleIAttenuation(); + pidData[axis].D *= getThrottleDAttenuation(); + } + const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D; pidData[axis].Sum = pidSum; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d7d424c608..7431cdd91a 100755 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -69,6 +69,13 @@ typedef enum { PID_CRASH_RECOVERY_BEEP } pidCrashRecovery_e; +typedef enum { + MIXER_IMPL_LEGACY = 0, + MIXER_IMPL_SMOOTH, + MIXER_IMPL_2PASS, + MIXER_IMPL_COUNT +} mixerImplType_e; + typedef struct pidf_s { uint8_t P; uint8_t I; @@ -130,10 +137,16 @@ typedef struct pidProfile_s { uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system uint8_t iterm_relax_cutoff; uint8_t iterm_relax_cutoff_yaw; - uint8_t iterm_relax_threshold; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint - uint8_t iterm_relax_threshold_yaw; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint + uint8_t iterm_relax_threshold; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint + uint8_t iterm_relax_threshold_yaw; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t motor_output_limit; // Upper limit of the motor output (percent) int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used + + uint8_t linear_thrust_low_output; // Sets the level of thrust linearization for low motor outputs + uint8_t linear_thrust_high_output; // Sets the level of thrust linearization for high motor outputs + uint8_t linear_throttle; // When thrust linearization is enabled, tells whether the throttle has to be linear or counter-compensated for legacy feedback + mixerImplType_e mixer_impl; // Which mixer implementation use + uint8_t mixer_laziness; // If enabled, mixer clipping strategy will shift values only by the minimum required amount per motor group. Requires linear thrust } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 3344ecf671..d4325ccb16 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -867,8 +867,9 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU8(dst, currentControlRateProfile->throttle_limit_percent); sbufWriteU8(dst, currentControlRateProfile->vbat_comp_type); sbufWriteU8(dst, currentControlRateProfile->vbat_comp_ref); - sbufWriteU8(dst, currentControlRateProfile->vbat_comp_throttle_level); - sbufWriteU8(dst, currentControlRateProfile->vbat_comp_pid_level); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + // sitckpids added in 1.46 sbufWriteU8(dst, currentControlRateProfile->rateDynamics.rateSensCenter); sbufWriteU8(dst, currentControlRateProfile->rateDynamics.rateSensEnd); @@ -1582,8 +1583,8 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { if (sbufBytesRemaining(src) >= 4) { currentControlRateProfile->vbat_comp_type = sbufReadU8(src); currentControlRateProfile->vbat_comp_ref = sbufReadU8(src); - currentControlRateProfile->vbat_comp_throttle_level = sbufReadU8(src); - currentControlRateProfile->vbat_comp_pid_level = sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); } if (sbufBytesRemaining(src) >= 6) { currentControlRateProfile->rateDynamics.rateSensCenter = sbufReadU8(src); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 034739523c..29dbe0269e 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -378,6 +378,10 @@ static const char * const lookupTableOsdLogoOnArming[] = { }; #endif +static const char *const lookupTableMixerImplType[] = { + "LEGACY", "SMOOTH", "2PASS" +}; + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -466,6 +470,7 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_OSD LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming), #endif + LOOKUP_TABLE_ENTRY(lookupTableMixerImplType), }; #undef LOOKUP_TABLE_ENTRY @@ -774,9 +779,7 @@ const clivalue_t valueTable[] = { { "throttle_limit_percent", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) }, { "vbat_comp_type", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_VBAT_COMP_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, vbat_comp_type) }, - { "vbat_comp_ref", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { VBAT_CELL_VOTAGE_RANGE_MIN, VBAT_CELL_VOTAGE_RANGE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, vbat_comp_ref) }, - { "vbat_comp_throttle_level", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, vbat_comp_throttle_level) }, - { "vbat_comp_pid_level", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, vbat_comp_pid_level) }, + { "vbat_comp_ref" , VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { VBAT_CELL_VOTAGE_RANGE_MIN, VBAT_CELL_VOTAGE_RANGE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, vbat_comp_ref) }, // PG_SERIAL_CONFIG { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, @@ -923,9 +926,14 @@ const clivalue_t valueTable[] = { { "horizon_tilt_effect", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 180 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) }, { "horizon_strength", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizonStrength) }, - { "motor_output_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { MOTOR_OUTPUT_LIMIT_PERCENT_MIN, MOTOR_OUTPUT_LIMIT_PERCENT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, motor_output_limit) }, - { "auto_profile_cell_count", VAR_INT8 | PROFILE_VALUE, .config.minmax = { AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT }, PG_PID_PROFILE, offsetof(pidProfile_t, auto_profile_cell_count) }, + { "motor_output_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { MOTOR_OUTPUT_LIMIT_PERCENT_MIN, MOTOR_OUTPUT_LIMIT_PERCENT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, motor_output_limit) }, + { "auto_profile_cell_count", VAR_INT8 | PROFILE_VALUE, .config.minmax = { AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT }, PG_PID_PROFILE, offsetof(pidProfile_t, auto_profile_cell_count) }, + { "linear_thrust_low_output", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, linear_thrust_low_output) }, + { "linear_thrust_high_output", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, linear_thrust_high_output) }, + { "linear_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, linear_throttle) }, + { "mixer_impl", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_IMPL_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_impl) }, + { "mixer_laziness", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_laziness) }, // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index d7d662e24b..8c073a4ac0 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -111,6 +111,7 @@ typedef enum { #ifdef USE_OSD TABLE_OSD_LOGO_ON_ARMING, #endif + TABLE_MIXER_IMPL_TYPE, LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 6e423c124b..738f5ec699 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -408,24 +408,28 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) { } } -float calculateVbatCompensation(uint8_t vbatCompType, uint8_t vbatCompRef) { - float factor = 1.0f; - if (vbatCompType != VBAT_COMP_TYPE_OFF && batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) { - float vbat = (float) voltageMeter.filtered / batteryCellCount; - if (vbat) { - factor = vbatCompRef / vbat; - factor *= factor; - switch (vbatCompType) { - case VBAT_COMP_TYPE_BOOST: - factor = MAX(factor, 1.0f); - break; - case VBAT_COMP_TYPE_LIMIT: - factor = MIN(factor, 1.0f); - break; - }; - } +float calculateBatteryCompensationFactor() +{ + if (currentControlRateProfile->vbat_comp_type == VBAT_COMP_TYPE_OFF + || batteryConfig()->voltageMeterSource == VOLTAGE_METER_NONE + || batteryCellCount == 0) { + return 1.0f; + } + + float vbat = (float) voltageMeter.filtered / batteryCellCount; + vbat = constrainf(vbat, batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatmaxcellvoltage); + + float factor = currentControlRateProfile->vbat_comp_ref / vbat; + + switch (currentControlRateProfile->vbat_comp_type) { + case VBAT_COMP_TYPE_BOOST: + return MAX(factor, 1.0f); + case VBAT_COMP_TYPE_LIMIT: + return MIN(factor, 1.0f); + case VBAT_COMP_TYPE_BOTH: // or it would be unused + default: + return factor; } - return factor; } uint8_t calculateBatteryPercentageRemaining(void) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 4275812438..d1d0086f26 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -97,7 +97,7 @@ void batteryUpdateAlarms(void); struct rxConfig_s; -float calculateVbatCompensation(uint8_t vbatCompType, uint8_t vbatCompRef); +float calculateBatteryCompensationFactor(); uint8_t calculateBatteryPercentageRemaining(void); bool isBatteryVoltageConfigured(void); uint16_t getBatteryVoltage(void); diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index ad394c340f..33447ea457 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -28,7 +28,7 @@ bool simulateMixerSaturated = false; float simulatedSetpointRate[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedThrottlePIDAttenuation = 1.0f; -float simulatedMotorMixRange = 0.0f; +float simulatedControllerMixRange = 0.0f; int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t debugMode; @@ -65,7 +65,7 @@ extern "C" { attitudeEulerAngles_t attitude; float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; } - float getMotorMixRange(void) { return simulatedMotorMixRange; } + float getControllerMixRange(void) { return simulatedControllerMixRange; } float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; } bool mixerIsOutputSaturated(int, float) { return simulateMixerSaturated; } float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); } @@ -128,7 +128,7 @@ void resetTest(void) { loopIter = 0; simulateMixerSaturated = false; simulatedThrottlePIDAttenuation = 1.0f; - simulatedMotorMixRange = 0.0f; + simulatedControllerMixRange = 0.0f; pidStabilisationState(PID_STABILISATION_OFF); DISABLE_ARMING_FLAG(ARMED); @@ -263,12 +263,12 @@ TEST(pidControllerTest, testPidLoop) { EXPECT_FLOAT_EQ(-132.25, pidData[FD_YAW].D); // Simulate Iterm behaviour during mixer saturation - simulatedMotorMixRange = 1.2f; + simulatedControllerMixRange = 1.2f; pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5)); ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6)); ASSERT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8)); - simulatedMotorMixRange = 0; + simulatedControllerMixRange = 0; // Match the stick to gyro to stop error simulatedSetpointRate[FD_ROLL] = 100; @@ -449,7 +449,7 @@ TEST(pidControllerTest, testCrashRecoveryMode) { // generate crash detection for roll axis gyro.gyroADCf[FD_ROLL] = 800; - simulatedMotorMixRange = 1.2f; + simulatedControllerMixRange = 1.2f; for (int loop =0; loop <= loopsToCrashTime; loop++) { gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL]; pidController(pidProfile, &rollAndPitchTrims, currentTestTime());