diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d7841d40e0..8aaba22fcb 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -726,7 +726,12 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t // 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. for (int i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle)); + float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle; +#ifdef USE_THRUST_LINEARIZATION + motorOutput = pidApplyThrustLinearization(motorOutput); +#endif + motorOutput = motorOutputMin + motorOutputRange * motorOutput; + #ifdef USE_SERVOS if (mixerIsTricopter()) { motorOutput += mixerTricopterMotorCorrection(i); @@ -887,6 +892,11 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa updateDynLpfCutoffs(currentTimeUs, throttle); #endif +#ifdef USE_THRUST_LINEARIZATION + // reestablish old throttle stick feel by counter compensating thrust linearization + throttle = pidCompensateThrustLinearization(throttle); +#endif + #if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9e2c496138..21d609baf4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 6); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 7); void resetPidProfile(pidProfile_t *pidProfile) { @@ -181,6 +181,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .launchControlAllowTriggerReset = true, .use_integrated_yaw = false, .integrated_yaw_relax = 200, + .thrustLinearization = 0, ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 150; @@ -474,6 +475,12 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pi static FAST_RAM_ZERO_INIT float acroTrainerGain; #endif // USE_ACRO_TRAINER +#ifdef USE_THRUST_LINEARIZATION +FAST_RAM_ZERO_INIT float thrustLinearization; +FAST_RAM_ZERO_INIT float thrustLinearizationReciprocal; +FAST_RAM_ZERO_INIT float thrustLinearizationB; +#endif + void pidUpdateAntiGravityThrottleFilter(float throttle) { if (antiGravityMode == ANTI_GRAVITY_SMOOTH) { @@ -597,6 +604,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) useIntegratedYaw = pidProfile->use_integrated_yaw; integratedYawRelax = pidProfile->integrated_yaw_relax; #endif + +#ifdef USE_THRUST_LINEARIZATION + thrustLinearization = pidProfile->thrustLinearization / 100.0f; + thrustLinearizationReciprocal = 1.0f / thrustLinearization; + thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * thrustLinearization); +#endif } void pidInit(const pidProfile_t *pidProfile) @@ -614,6 +627,27 @@ void pidAcroTrainerInit(void) } #endif // USE_ACRO_TRAINER +#ifdef USE_THRUST_LINEARIZATION +float pidCompensateThrustLinearization(float throttle) +{ + if (thrustLinearization != 0.0f) { + throttle = throttle * (throttle * thrustLinearization + 1.0f - thrustLinearization); + } + return throttle; +} + +float pidApplyThrustLinearization(float motorOutput) +{ + if (thrustLinearization != 0.0f) { + if (motorOutput > 0.0f) { + motorOutput = sqrtf(motorOutput * thrustLinearizationReciprocal + + thrustLinearizationB * thrustLinearizationB) - thrustLinearizationB; + } + } + return motorOutput; +} +#endif + void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex) { if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 51bbc76920..e653bab540 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -159,6 +159,7 @@ typedef struct pidProfile_s { uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset uint8_t use_integrated_yaw; // Selects whether the yaw pidsum should integrated uint8_t integrated_yaw_relax; // Specifies how much integrated yaw should be reduced to offset the drag based yaw component + uint8_t thrustLinearization; // Compensation factor for pid linearization } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); @@ -210,6 +211,10 @@ bool pidOsdAntiGravityActive(void); bool pidOsdAntiGravityMode(void); void pidSetAntiGravityState(bool newState); bool pidAntiGravityEnabled(void); +#ifdef USE_THRUST_LINEARIZATION +float pidApplyThrustLinearization(float motorValue); +float pidCompensateThrustLinearization(float throttle); +#endif #ifdef UNIT_TEST #include "sensors/acceleration.h" diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 90d39c1828..b0aa94735a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -954,6 +954,10 @@ const clivalue_t valueTable[] = { { "launch_control_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlGain) }, #endif +#ifdef USE_THRUST_LINEARIZATION + { "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) }, +#endif + // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index d3539d8b91..9039b9f191 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -23,6 +23,7 @@ #ifdef FURYF3OSD #define TARGET_BOARD_IDENTIFIER "FY3O" // #define USBD_PRODUCT_STRING "FuryF3OSD" + #undef USE_THRUST_LINEARIZATION #else #define TARGET_BOARD_IDENTIFIER "FYF3" // #define USBD_PRODUCT_STRING "FuryF3" diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index fca5041420..136862bc8e 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -31,6 +31,7 @@ #undef USE_ITERM_RELAX #undef USE_RC_SMOOTHING_FILTER +#undef USE_THRUST_LINEARIZATION #undef USE_HUFFMAN #undef USE_PINIO diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 10d41bf7d7..0e9da42831 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -58,6 +58,7 @@ #undef USE_EXTENDED_CMS_MENUS #endif +#undef USE_THRUST_LINEARIZATION #undef USE_BOARD_INFO #undef USE_RTC_TIME diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index c4f66c3e04..40fd214359 100644 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -53,6 +53,7 @@ #undef USE_SERIALRX_SUMH #undef USE_PWM +#undef USE_THRUST_LINEARIZATION #undef USE_BOARD_INFO #undef USE_EXTENDED_CMS_MENUS #undef USE_RTC_TIME diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 776342dc59..41a20f295b 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -199,6 +199,7 @@ #define USE_ITERM_RELAX #define USE_DYN_LPF #define USE_INTEGRATED_YAW_CONTROL +#define USE_THRUST_LINEARIZATION #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND