TPA gravity factor (for wings) (#13778)

* Wing TPA gravity factor

* Small corrections according to ledvinap's review (not all yet)

* Some review changes for tpa_gravity

* using throttle after LPF to scale between thr0 and thr100

* tpa gravity further review and fixes

* tpa gravity: removed logging of pitch angle, since it's just pitch

* moved tpa_gravity to the end in pid.h

* KarateBrot review

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:

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) },

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)

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);

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"

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];

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);

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;
 }
 

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;
 

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)
     }