First draft of better horizon mode

src/main/config/config.c

@@ -159,6 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->D_f[YAW] = 0.05f;
     pidProfile->A_level = 5.0f;
     pidProfile->H_level = 3.0f;
+    pidProfile->H_sensitivity = 10.0f;
 }
 
 #ifdef GPS

src/main/flight/flight.c

@@ -101,9 +101,28 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
     float delta, deltaSum;
     float dT;
     int axis;
+    float horizonLevelStrength = 1;
 
     dT = (float)cycleTime * 0.000001f;
 
+    if (FLIGHT_MODE(HORIZON_MODE)) {
+
+        if(abs(rcCommand[FD_ROLL]) > abs(rcCommand[FD_PITCH])){
+            axis = FD_ROLL;
+        }
+        else {
+            axis = FD_PITCH;
+        }
+
+        // Progressively turn off the horizon self level strength as the stick is banged over
+        horizonLevelStrength = (float)(500 - abs(rcCommand[axis])) / 500;  // 1 at centre stick, 0 = max stick deflection
+        if(pidProfile->H_sensitivity == 0){
+            horizonLevelStrength = 0;
+        } else {
+            horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (10 / pidProfile->H_sensitivity)) + 1, 0, 1);
+        }
+    }
+
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
         // -----Get the desired angle rate depending on flight mode
@@ -134,7 +153,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
                 AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
                 if (FLIGHT_MODE(HORIZON_MODE)) {
                     // mix up angle error to desired AngleRate to add a little auto-level feel
-                    AngleRate += errorAngle * pidProfile->H_level;
+                    AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
                 }
             }
         }

src/main/flight/flight.h

@@ -42,6 +42,7 @@ typedef struct pidProfile_s {
     float D_f[3];
     float A_level;
     float H_level;
+    float H_sensitivity;
 } pidProfile_t;
 
 typedef enum {

src/main/io/serial_cli.c

@@ -392,6 +392,7 @@ const clivalue_t valueTable[] = {
 
     { "level_horizon",              VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 },
     { "level_angle",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 },
+    { "sensitivity_horizon",        VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 },
 
     { "p_alt",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 },
     { "i_alt",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 },

src/main/io/serial_msp.c

@@ -846,7 +846,7 @@ static bool processOutCommand(uint8_t cmdMSP)
                 if (i == PIDLEVEL) {
                     serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
                     serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
-                    serialize8(0);
+                    serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity * 10.0f), 0, 250));
                 } else {
                     serialize8(currentProfile->pidProfile.P8[i]);
                     serialize8(currentProfile->pidProfile.I8[i]);
@@ -1165,7 +1165,7 @@ static bool processInCommand(void)
                 if (i == PIDLEVEL) {
                     currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
                     currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
-                    read8();
+                    currentProfile->pidProfile.H_sensitivity = (float)read8() / 10.0f;
                 } else {
                     currentProfile->pidProfile.P8[i] = read8();
                     currentProfile->pidProfile.I8[i] = read8();