Merge pull request #351 from nebbian/baseflight-pid-horizon-mode-tuneup_a

Baseflight pid (pid_controller=2) horizon mode tuneup

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 = 75;
 }
 
 #ifdef GPS

src/main/flight/flight.c

@@ -101,14 +101,38 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
 {
     float RateError, errorAngle, AngleRate, gyroRate;
     float ITerm,PTerm,DTerm;
+    int32_t stickPosAil, stickPosEle, mostDeflectedPos;
     static float lastGyroRate[3];
     static float delta1[3], delta2[3];
     float delta, deltaSum;
     float dT;
     int axis;
+    float horizonLevelStrength = 1;
 
     dT = (float)cycleTime * 0.000001f;
 
+    if (FLIGHT_MODE(HORIZON_MODE)) {
+
+        // Figure out the raw stick positions
+        stickPosAil = getRcStickPosition(FD_ROLL);
+        stickPosEle = getRcStickPosition(FD_PITCH);
+
+        if(abs(stickPosAil) > abs(stickPosEle)){
+            mostDeflectedPos = abs(stickPosAil);
+        }
+        else {
+            mostDeflectedPos = abs(stickPosEle);
+        }
+
+        // Progressively turn off the horizon self level strength as the stick is banged over
+        horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection
+        if(pidProfile->H_sensitivity == 0){
+            horizonLevelStrength = 0;
+        } else {
+            horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1);
+        }
+    }
+
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
         // -----Get the desired angle rate depending on flight mode
@@ -139,7 +163,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;
+    uint8_t H_sensitivity;
 } pidProfile_t;
 
 typedef enum {

src/main/io/serial_cli.c

@@ -393,6 +393,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_UINT8  | 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), 0, 250));
                 } else {
                     serialize8(currentProfile->pidProfile.P8[i]);
                     serialize8(currentProfile->pidProfile.I8[i]);
@@ -1171,7 +1171,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 = read8();
                 } else {
                     currentProfile->pidProfile.P8[i] = read8();
                     currentProfile->pidProfile.I8[i] = read8();

src/main/mw.c

@@ -355,6 +355,11 @@ void mwArm(void)
     }
 }
 
+int32_t getRcStickPosition(int32_t axis) {
+
+    return min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
+}
+
 // Automatic ACC Offset Calibration
 bool AccInflightCalibrationArmed = false;
 bool AccInflightCalibrationMeasurementDone = false;

src/main/mw.h

@@ -22,3 +22,5 @@ void handleInflightCalibrationStickPosition();
 
 void mwDisarm(void);
 void mwArm(void);
+
+int32_t getRcStickPosition(int32_t axis);