diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index bbda754744f..c7bc789be5d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1687,6 +1687,8 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_WING BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS, "%d", currentPidProfile->tpa_delay_ms); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR0, "%d", currentPidProfile->tpa_gravity_thr0); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR100, "%d", currentPidProfile->tpa_gravity_thr100); #endif default: diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 91d18fbb39d..15b004c9a29 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1267,6 +1267,8 @@ const clivalue_t valueTable[] = { #ifdef USE_WING { PARAM_NAME_TPA_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_delay_ms) }, + { PARAM_NAME_TPA_GRAVITY_THR0, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr0) }, + { PARAM_NAME_TPA_GRAVITY_THR100, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr100) }, #endif { PARAM_NAME_EZ_LANDING_THRESHOLD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_threshold) }, diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 93c62d660fb..99ddc7cacd3 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -105,6 +105,12 @@ float acos_approx(float x) else return result; } + +float asin_approx(float x) +{ + return (M_PIf * 0.5f) - acos_approx(x); +} + #endif int gcd(int num, int denom) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 9e60099339f..b4223389b6c 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -135,6 +135,7 @@ float sin_approx(float x); float cos_approx(float x); float atan2_approx(float y, float x); float acos_approx(float x); +float asin_approx(float x); #define tan_approx(x) (sin_approx(x) / cos_approx(x)) float exp_approx(float val); float log_approx(float val); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 3f3ac448c5f..1c09f871a16 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -59,6 +59,8 @@ #define PARAM_NAME_TPA_LOW_ALWAYS "tpa_low_always" #define PARAM_NAME_TPA_MODE "tpa_mode" #define PARAM_NAME_TPA_DELAY_MS "tpa_delay_ms" +#define PARAM_NAME_TPA_GRAVITY_THR0 "tpa_gravity_thr0" +#define PARAM_NAME_TPA_GRAVITY_THR100 "tpa_gravity_thr100" #define PARAM_NAME_MIXER_TYPE "mixer_type" #define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold" #define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit" diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index aea3f1a88fb..18735e19259 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -749,6 +749,13 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) } #endif // USE_ACC +// Angle in between the nose axis of the craft and the horizontal plane in ground reference. +// Positive angle - nose down, negative angle - nose up. +float getSinPitchAngle(void) +{ + return -rMat[2][0]; +} + float getCosTiltAngle(void) { return rMat[2][2]; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9e7c6627a2c..8dd758dd063 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -69,6 +69,7 @@ typedef struct imuRuntimeConfig_s { void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value); +float getSinPitchAngle(void); float getCosTiltAngle(void); void getQuaternion(quaternion * q); void imuUpdateAttitude(timeUs_t currentTimeUs); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 704af23bb5d..1c845f56687 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -231,6 +231,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .ez_landing_limit = 15, .ez_landing_speed = 50, .tpa_delay_ms = 0, + .tpa_gravity_thr0 = 0, + .tpa_gravity_thr100 = 0, .spa_center = { 0, 0, 0 }, .spa_width = { 0, 0, 0 }, .spa_mode = { 0, 0, 0 }, @@ -281,32 +283,48 @@ void pidResetIterm(void) } } +#ifdef USE_WING +static float getWingTpaArgument(float throttle) +{ + const float pitchFactorAdjustment = scaleRangef(throttle, 0.0f, 1.0f, pidRuntime.tpaGravityThr0, pidRuntime.tpaGravityThr100); + const float pitchAngleFactor = getSinPitchAngle() * pitchFactorAdjustment; + DEBUG_SET(DEBUG_TPA, 1, lrintf(pitchAngleFactor * 1000.0f)); + + float tpaArgument = throttle + pitchAngleFactor; + const float maxTpaArgument = MAX(1.0 + pidRuntime.tpaGravityThr100, pidRuntime.tpaGravityThr0); + tpaArgument = tpaArgument / maxTpaArgument; + tpaArgument = pt2FilterApply(&pidRuntime.tpaLpf, tpaArgument); + DEBUG_SET(DEBUG_TPA, 2, lrintf(tpaArgument * 1000.0f)); + + return tpaArgument; +} +#endif // #ifndef USE_WING + void pidUpdateTpaFactor(float throttle) { static bool isTpaLowFaded = false; // don't permit throttle > 1 & throttle < 0 ? is this needed ? can throttle be > 1 or < 0 at this point throttle = constrainf(throttle, 0.0f, 1.0f); - bool isThrottlePastTpaLowBreakpoint = (throttle < pidRuntime.tpaLowBreakpoint && pidRuntime.tpaLowBreakpoint > 0.01f) ? false : true; + +#ifdef USE_WING + const float tpaArgument = isFixedWing() ? getWingTpaArgument(throttle) : throttle; +#else + const float tpaArgument = throttle; +#endif + + bool isThrottlePastTpaLowBreakpoint = (tpaArgument >= pidRuntime.tpaLowBreakpoint || pidRuntime.tpaLowBreakpoint <= 0.01f); float tpaRate = 0.0f; if (isThrottlePastTpaLowBreakpoint || isTpaLowFaded) { - tpaRate = pidRuntime.tpaMultiplier * fmaxf(throttle - pidRuntime.tpaBreakpoint, 0.0f); + tpaRate = pidRuntime.tpaMultiplier * fmaxf(tpaArgument - pidRuntime.tpaBreakpoint, 0.0f); if (!pidRuntime.tpaLowAlways && !isTpaLowFaded) { isTpaLowFaded = true; } } else { - tpaRate = pidRuntime.tpaLowMultiplier * (pidRuntime.tpaLowBreakpoint - throttle); + tpaRate = pidRuntime.tpaLowMultiplier * (pidRuntime.tpaLowBreakpoint - tpaArgument); } float tpaFactor = 1.0f - tpaRate; DEBUG_SET(DEBUG_TPA, 0, lrintf(tpaFactor * 1000)); - -#ifdef USE_WING - if (isFixedWing()) { - tpaFactor = pt2FilterApply(&pidRuntime.tpaLpf, tpaFactor); - DEBUG_SET(DEBUG_TPA, 1, lrintf(tpaFactor * 1000)); - } -#endif - pidRuntime.tpaFactor = tpaFactor; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 0fb26825b87..76a6001ff54 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -72,6 +72,7 @@ #ifdef USE_WING #define TPA_LOW_RATE_MIN INT8_MIN +#define TPA_GRAVITY_MAX 5000 #else #define TPA_LOW_RATE_MIN 0 #endif @@ -264,6 +265,8 @@ typedef struct pidProfile_s { uint16_t spa_center[XYZ_AXIS_COUNT]; // RPY setpoint at which PIDs are reduced to 50% (setpoint PID attenuation) uint16_t spa_width[XYZ_AXIS_COUNT]; // Width of smooth transition around spa_center uint8_t spa_mode[XYZ_AXIS_COUNT]; // SPA mode for each axis + uint16_t tpa_gravity_thr0; // For wings: addition to tpa argument in % when zero throttle + uint16_t tpa_gravity_thr100; // For wings: addition to tpa argument in % when full throttle } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -447,6 +450,8 @@ typedef struct pidRuntime_s { #ifdef USE_WING pt2Filter_t tpaLpf; float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation + float tpaGravityThr0; + float tpaGravityThr100; #endif } pidRuntime_t; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 0fe69925bd9..b17db759efc 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -259,6 +259,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT)); #ifdef USE_WING pt2FilterInit(&pidRuntime.tpaLpf, pt2FilterGainFromDelay(pidProfile->tpa_delay_ms / 1000.0f, pidRuntime.dT)); + pidRuntime.tpaGravityThr0 = pidProfile->tpa_gravity_thr0 / 100.0f; + pidRuntime.tpaGravityThr100 = pidProfile->tpa_gravity_thr100 / 100.0f; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pidRuntime.spa[axis] = 1.0f; // 1.0 = no PID attenuation in runtime. 0 - full attenuation (no PIDs) }