MAG_HOLD diff limiting and MAG controller refactoring (#195)

* MAGHOLD constrained to 30 degrees
* mag_hold_heading_diff_limit CLI setting added to allow custom MAG_HOLD diff limit
* magHeading moved to pid.c and real interface created to inject data into it
* mag hold controller with current logic moved to pid.c
* 2Hz LPF filter added to magHold error to smoothen controller response
* variable name changed to clearly state its purpose
* LPF filter in MAG_HOLD controller moved after error limiting
* yaw_control_direction passed to pidController as argument, mag_hold_heading_diff_limit moved to pidProfile
* yaw_control_direction setting removed
* pidMagHold translates directly to rotation rate in dps
* pidMagHold refactored to the new principles and limiting
* name of filter changed to reflect its purpose
* HEADING_LOCK mode can be used only when MAG_HOLD is not enabled
* compiler warning supressed
* new MAG_HOLD defaults, RATE_LIMIT increased to 80dps, MAG_HOLD P to 60
* mag_rate_limit 90dps

docs/Cli.md

@@ -184,7 +184,6 @@ Re-apply any new defaults as desired.
 | `yaw_deadband`                  | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle.                                                                                                                                                                                                                                                                                                                                                                                              | 0      | 100    | 0             | Profile      | UINT8    |
 | `throttle_correction_value`     | The throttle_correction_value will be added to the throttle input. It will be maximal at the throttle_correction_angle and over, null when the copter is leveled and proportional in bewteen. The angle is set with 0.1 deg steps from 1 to 900, ie : 300 = 30.0 deg, 225 = 22.5 deg.                                                                                                                                                                                                                                                                                                                                                                  | 0      | 150    | 0             | Profile      | UINT8    |
 | `throttle_correction_angle`     | The throttle_correction_value will be added to the throttle input. It will be maximal at the throttle_correction_angle and over, null when the copter is leveled and proportional in bewteen. The angle is set with 0.1 deg steps from 1 to 900, ie : 300 = 30.0 deg, 225 = 22.5 deg.                                                                                                                                                                                                                                                                                                                                                                  | 1      | 900    | 800           | Profile      | UINT16   |
-| `yaw_control_direction`         |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | -1     | 1      | 1             | Master       | INT8     |
 | `yaw_motor_direction`                 |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  | -1     | 1      | 1             | Profile      | INT8     |
 | `tri_unarmed_servo`             | On tricopter mix only, if this is set to 1, servo will always be correcting regardless of armed state. to disable this, set it to 0.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   | OFF    | ON     | ON            | Profile      | INT8     |
 | `default_rate_profile`          | Default = profile number                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               | 0      | 2      |               | Profile      | UINT8    |
@@ -252,3 +251,5 @@ Re-apply any new defaults as desired.
 | `yaw_p_limit`                   | Limiter for yaw P term. This parameter is only affecting PID controller MW23. To disable set to 500 (actual default).                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   | 100    | 500    | 500           | Profile      | UINT16   |
 | `blackbox_rate_num`             |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 1      | 32     | 1             | Master       | UINT8    |
 | `blackbox_rate_denom`           |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 1      | 32     | 1             | Master       | UINT8    |    
+| `mag_hold_rate_limit`   | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. 
+| 10     | 255    | 40           | Profile       | UINT8    |

src/main/config/config.c

@@ -173,7 +173,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->P8[PIDLEVEL] = 120; // Self-level strength * 40 (4 * 40)
     pidProfile->I8[PIDLEVEL] = 15;  // Self-leveing low-pass frequency (0 - disabled)
     pidProfile->D8[PIDLEVEL] = 75;  // 75% horizon strength
-    pidProfile->P8[PIDMAG] = 40;
+    pidProfile->P8[PIDMAG] = 60;
     pidProfile->P8[PIDVEL] = 100;   // NAV_VEL_Z_P * 100
     pidProfile->I8[PIDVEL] = 50;    // NAV_VEL_Z_I * 100
     pidProfile->D8[PIDVEL] = 10;    // NAV_VEL_Z_D * 100
@@ -184,6 +184,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->yaw_lpf_hz = 30;
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
+    pidProfile->mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT;
 
     pidProfile->max_angle_inclination[FD_ROLL] = 300;    // 30 degrees
     pidProfile->max_angle_inclination[FD_PITCH] = 300;    // 30 degrees
@@ -451,7 +452,6 @@ static void resetConf(void)
     masterConfig.boardAlignment.pitchDeciDegrees = 0;
     masterConfig.boardAlignment.yawDeciDegrees = 0;
     masterConfig.acc_hardware = ACC_DEFAULT;     // default/autodetect
-    masterConfig.yaw_control_direction = 1;
     masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
 
     masterConfig.mag_hardware = MAG_DEFAULT;     // default/autodetect

src/main/config/config_master.h

@@ -48,7 +48,6 @@ typedef struct master_t {
     sensorAlignmentConfig_t sensorAlignmentConfig;
     boardAlignment_t boardAlignment;
 
-    int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
 
     uint16_t dcm_kp_acc;                    // DCM filter proportional gain ( x 10000) for accelerometer

src/main/flight/navigation_rewrite_fixedwing.c

@@ -48,11 +48,6 @@
 static bool isPitchAndThrottleAdjustmentValid = false;
 static bool isRollAdjustmentValid = false;
 
-/*-----------------------------------------------------------
- * Backdoor to MW heading controller
- *-----------------------------------------------------------*/
-extern int16_t magHold;
-
 /*-----------------------------------------------------------
  * Altitude controller
  *-----------------------------------------------------------*/
@@ -265,7 +260,7 @@ static void updatePositionHeadingController_FW(uint32_t deltaMicros)
 
     // Update magHold heading lock in case pilot is using MAG mode (prevent MAGHOLD to fight navigation)
     posControl.desiredState.yaw = wrap_36000(posControl.actualState.yaw + headingError);
-    magHold = CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
 
     // Add pitch compensation
     //posControl.rcAdjustment[PITCH] = -CENTIDEGREES_TO_DECIDEGREES(ABS(rollAdjustment)) * 0.50f;
@@ -382,7 +377,7 @@ void calculateFixedWingInitialHoldPosition(t_fp_vector * pos)
 
 void resetFixedWingHeadingController(void)
 {
-    magHold = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
 }
 
 //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY

src/main/flight/navigation_rewrite_multicopter.c

@@ -50,11 +50,6 @@
 
 #if defined(NAV)
 
-/*-----------------------------------------------------------
- * Backdoor to MW heading controller
- *-----------------------------------------------------------*/
-extern int16_t magHold;
-
 /*-----------------------------------------------------------
  * Altitude controller for multicopter aircraft
  *-----------------------------------------------------------*/
@@ -578,12 +573,12 @@ void calculateMulticopterInitialHoldPosition(t_fp_vector * pos)
 
 void resetMulticopterHeadingController(void)
 {
-    magHold = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
 }
 
 static void applyMulticopterHeadingController(void)
 {
-    magHold = CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
 }
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)

src/main/flight/pid.c

@@ -47,6 +47,7 @@
 #include "flight/imu.h"
 #include "flight/navigation_rewrite.h"
 
+#define MAG_HOLD_ERROR_LPF_FREQ 2
 
 typedef struct {
     float kP;
@@ -80,6 +81,8 @@ extern uint8_t motorCount;
 extern bool motorLimitReached;
 extern float dT;
 
+int16_t magHoldTargetHeading;
+
 // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
 static float tpaFactor;
 int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
@@ -293,13 +296,127 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
 #endif
 }
 
+void updateMagHoldHeading(int16_t heading)
+{
+    magHoldTargetHeading = heading;
+}
+
+int16_t getMagHoldHeading() {
+    return magHoldTargetHeading;
+}
+
+uint8_t getMagHoldState()
+{
+
+    #ifndef MAG
+        return MAG_HOLD_DISABLED;
+    #endif
+
+    if (!sensors(SENSOR_MAG) || !STATE(SMALL_ANGLE)) {
+        return MAG_HOLD_DISABLED;
+    }
+
+#if defined(NAV)
+    int navHeadingState = naivationGetHeadingControlState();
+    // NAV will prevent MAG_MODE from activating, but require heading control
+    if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
+        // Apply maghold only if heading control is in auto mode
+        if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {
+            return MAG_HOLD_ENABLED;
+        }
+    }
+    else
+#endif
+    if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
+        return MAG_HOLD_ENABLED;
+    } else {
+        return MAG_HOLD_UPDATE_HEADING;
+    }
+
+    return MAG_HOLD_UPDATE_HEADING;
+}
+
+/*
+ * MAG_HOLD P Controller returns desired rotation rate in dps to be fed to Rate controller
+ */
+float pidMagHold(const pidProfile_t *pidProfile)
+{
+
+    static filterStatePt1_t magHoldRateFilter;
+    float magHoldRate;
+
+    int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHoldTargetHeading;
+
+    /*
+     * Convert absolute error into relative to current heading
+     */
+    if (error <= -180) {
+        error += 360;
+    }
+
+    if (error >= +180) {
+        error -= 360;
+    }
+
+    /*
+        New MAG_HOLD controller work slightly different that previous one.
+        Old one mapped error to rotation speed in following way:
+            - on rate 0 it gave about 0.5dps for each degree of error
+            - error 0 = rotation speed of 0dps
+            - error 180 = rotation speed of 96 degrees per second
+            - output
+            - that gives about 2 seconds to correct any error, no matter how big. Of course, usually more because of inertia.
+        That was making him quite "soft" for small changes and rapid for big ones that started to appear
+        when iNav introduced real RTH and WAYPOINT that might require rapid heading changes.
+
+        New approach uses modified principle:
+            - manual yaw rate is not used. MAG_HOLD is decoupled from manual input settings
+            - instead, mag_hold_rate_limit is used. It defines max rotation speed in dps that MAG_HOLD controller can require from RateController
+            - computed rotation speed is capped at -mag_hold_rate_limit and mag_hold_rate_limit
+            - Default mag_hold_rate_limit = 40dps and default MAG_HOLD P-gain is 40
+            - With those values, maximum rotation speed will be required from Rate Controller when error is greater that 30 degrees
+            - For smaller error, required rate will be proportional.
+            - It uses LPF filter set at 2Hz to additionally smoothen out any rapid changes
+            - That makes correction of smaller errors stronger, and those of big errors softer
+
+        This make looks as very slow rotation rate, but please remember this is automatic mode.
+        Manual override with YAW input when MAG_HOLD is enabled will still use "manual" rates, not MAG_HOLD rates.
+        Highest possible correction is 180 degrees and it will take more less 4.5 seconds. It is even more than sufficient
+        to run RTH or WAYPOINT missions. My favourite rate range here is 20dps - 30dps that gives nice and smooth turns.
+
+        Correction for small errors is much faster now. For example, old contrioller for 2deg errors required 1dps (correction in 2 seconds).
+        New controller for 2deg error requires 2,6dps. 4dps for 3deg and so on up until mag_hold_rate_limit is reached.
+    */
+
+    magHoldRate = error * pidProfile->P8[PIDMAG] / 30;
+    magHoldRate = constrainf(magHoldRate, -pidProfile->mag_hold_rate_limit, pidProfile->mag_hold_rate_limit);
+    magHoldRate = filterApplyPt1(magHoldRate, &magHoldRateFilter, MAG_HOLD_ERROR_LPF_FREQ, dT);
+
+    return magHoldRate;
+}
+
 void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig)
 {
+
+    uint8_t magHoldState = getMagHoldState();
+
+    if (magHoldState == MAG_HOLD_UPDATE_HEADING) {
+        updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+    }
+
     for (int axis = 0; axis < 3; axis++) {
         // Step 1: Calculate gyro rates
         pidState[axis].gyroRate = gyroADC[axis] * gyro.scale;
-        // Step 2: Read sticks
+
-        const float rateTarget = pidRcCommandToRate(rcCommand[axis], controlRateConfig->rates[axis]);
+        // Step 2: Read target
+        float rateTarget;
+
+        if (axis == FD_YAW && magHoldState == MAG_HOLD_ENABLED) {
+            rateTarget = pidMagHold(pidProfile);
+        } else {
+            rateTarget = pidRcCommandToRate(rcCommand[axis], controlRateConfig->rates[axis]);
+        }
+
         // Limit desired rate to something gyro can measure reliably
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
     }
@@ -311,7 +428,7 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
         pidLevel(pidProfile, &pidState[FD_PITCH], FD_PITCH, horizonLevelStrength);
     }
 
-    if (FLIGHT_MODE(HEADING_LOCK)) {
+    if (FLIGHT_MODE(HEADING_LOCK) && magHoldState != MAG_HOLD_ENABLED) {
         pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]);
     }
 

src/main/flight/pid.h

@@ -28,6 +28,10 @@
 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
 #define YAW_P_LIMIT_MAX 300                 // Maximum value for yaw P limiter
 
+#define MAG_HOLD_RATE_LIMIT_MIN 10
+#define MAG_HOLD_RATE_LIMIT_MAX 250
+#define MAG_HOLD_RATE_LIMIT_DEFAULT 90
+
 typedef enum {
     PIDROLL,
     PIDPITCH,
@@ -56,6 +60,9 @@ typedef struct pidProfile_s {
     uint8_t yaw_lpf_hz;
 
     int16_t max_angle_inclination[ANGLE_INDEX_COUNT];       // Max possible inclination (roll and pitch axis separately
+
+    uint8_t mag_hold_rate_limit;    //Maximum rotation rate MAG_HOLD mode cas feed to yaw rate PID controller
+
 } pidProfile_t;
 
 extern int16_t axisPID[];
@@ -65,3 +72,12 @@ void pidResetErrorAccumulators(void);
 void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig);
 int16_t pidAngleToRcCommand(float angleDeciDegrees);
 void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig);
+
+enum {
+    MAG_HOLD_DISABLED = 0,
+    MAG_HOLD_UPDATE_HEADING,
+    MAG_HOLD_ENABLED
+};
+
+void updateMagHoldHeading(int16_t heading);
+int16_t getMagHoldHeading();

src/main/io/serial_cli.c

@@ -664,7 +664,6 @@ const clivalue_t valueTable[] = {
 
     { "throttle_tilt_comp_str",     VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].throttle_tilt_compensation_strength, .config.minmax = { 0,  100 }, 0 },
 
-    { "yaw_control_direction",      VAR_INT8   | MASTER_VALUE,  &masterConfig.yaw_control_direction, .config.minmax = { -1,  1 }, 0 },
     { "yaw_motor_direction",        VAR_INT8   | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1,  1 }, 0 },
     { "yaw_jump_prevention_limit",  VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW,  YAW_JUMP_PREVENTION_LIMIT_HIGH }, 0 },
 
@@ -707,6 +706,7 @@ const clivalue_t valueTable[] = {
 
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, .config.minmax = { 0,  MAG_MAX }, 0 },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000,  18000 }, 0 },
+    { "mag_hold_rate_limit",        VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.mag_hold_rate_limit, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN,  MAG_HOLD_RATE_LIMIT_MAX }, 0 },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0,  200 }, 0 },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0,  200 }, 0 },

src/main/io/serial_msp.c

@@ -866,7 +866,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize8(NAV_Status.activeWpNumber);
         serialize8(NAV_Status.error);
         //serialize16( (int16_t)(target_bearing/100));
-        serialize16(magHold);
+        serialize16(getMagHoldHeading());
         break;
     case MSP_WP:
         msp_wp_no = read8();    // get the wp number
@@ -1136,7 +1136,7 @@ static bool processInCommand(void)
         }
         break;
     case MSP_SET_HEAD:
-        magHold = read16();
+        updateMagHoldHeading(read16());
         break;
     case MSP_SET_RAW_RC:
         {

src/main/mw.c

@@ -101,7 +101,6 @@ uint16_t cycleTime = 0;         // this is the number in micro second to achieve
 
 float dT;
 
-int16_t magHold;
 int16_t headFreeModeHold;
 
 uint8_t motorControlEnable = false;
@@ -152,7 +151,7 @@ void annexCode(void)
                 }
             }
             tmp2 = tmp / 100;
-            rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
+            rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -1;
         }
 
         if (rcData[axis] < masterConfig.rxConfig.midrc)
@@ -278,46 +277,6 @@ void mwArm(void)
     }
 }
 
-void applyMagHold(void)
-{
-    int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
-
-    if (dif <= -180) {
-        dif += 360;
-    }
-
-    if (dif >= +180) {
-        dif -= 360;
-    }
-
-    dif *= masterConfig.yaw_control_direction;
-
-    if (STATE(SMALL_ANGLE)) {
-        rcCommand[YAW] = dif * currentProfile->pidProfile.P8[PIDMAG] / 30;
-    }
-}
-
-void updateMagHold(void)
-{
-#if defined(NAV)
-    int navHeadingState = naivationGetHeadingControlState();
-    // NAV will prevent MAG_MODE from activating, but require heading control
-    if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
-        // Apply maghold only if heading control is in auto mode
-        if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {
-            applyMagHold();
-        }
-    }
-    else
-#endif
-    if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
-        applyMagHold();
-    }
-    else {
-        magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
-    }
-}
-
 void processRx(void)
 {
     static bool armedBeeperOn = false;
@@ -440,7 +399,7 @@ void processRx(void)
         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
             if (!FLIGHT_MODE(MAG_MODE)) {
                 ENABLE_FLIGHT_MODE(MAG_MODE);
-                magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
+                updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
             }
         } else {
             DISABLE_FLIGHT_MODE(MAG_MODE);
@@ -589,12 +548,6 @@ void taskMainPidLoop(void)
     applyWaypointNavigationAndAltitudeHold();
 #endif
 
-#ifdef MAG
-    if (sensors(SENSOR_MAG)) {
-        updateMagHold();
-    }
-#endif
-
     // If we're armed, at minimum throttle, and we do arming via the
     // sticks, do not process yaw input from the rx.  We do this so the
     // motors do not spin up while we are trying to arm or disarm.

src/main/mw.h

@@ -17,8 +17,6 @@
 
 #pragma once
 
-extern int16_t magHold;
-
 void handleInflightCalibrationStickPosition();
 
 void mwDisarm(void);

src/test/unit/serial_msp_unittest.cc

@@ -544,7 +544,6 @@ void loadCustomServoMixer(void) {}
 void rxMspFrameReceive(uint16_t *frame, int channelCount) {UNUSED(frame);UNUSED(channelCount);}
 // from mw.c
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
-int16_t magHold;
 // from navigation.c
 int32_t GPS_home[2];
 int32_t GPS_hold[2];
@@ -578,4 +577,3 @@ uint16_t averageSystemLoadPercent = 0;
 // from transponder_ir.c
 void transponderUpdateData(uint8_t*) {}
 }
-