Merge pull request #11529 from breadoven/abo_magless_heading_zero

Allow yaw reset to north when disarmed - for multirotors with no compass

Author: breadoven

docs/Controls.md

@@ -31,7 +31,7 @@ The stick positions are combined to activate different functions:
 | Battery profile 3             | HIGH     | LOW     | CENTER | HIGH   |
 | Calibrate Gyro                | LOW      | LOW     | LOW    | CENTER |
 | Calibrate Acc                 | HIGH     | LOW     | LOW    | CENTER |
-| Calibrate Mag/Compass         | HIGH     | HIGH    | LOW    | CENTER |
+| Calibrate Compass/Zero Yaw    | HIGH     | HIGH    | LOW    | CENTER |
 | Trim Acc Left                 | HIGH     | CENTER  | CENTER | LOW    |
 | Trim Acc Right                | HIGH     | CENTER  | CENTER | HIGH   |
 | Trim Acc Forwards             | HIGH     | CENTER  | HIGH   | CENTER |
@@ -52,6 +52,14 @@ The stick positions are combined to activate different functions:
 For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/).
 ![Stick Positions](assets/images/StickPositions.png)
 
+## Compass Calibration and Yaw Zero Reset
+
+The stick function `Calibrate Compass/Zero Yaw` provides 2 functions depending on whether or not a compass is available.
+
+If a compass is available the stick function initiates the compass calibration routine.
+
+If no compass is available the stick function will reset the current yaw/heading estimate to zero (North) and also set the heading as trusted. This is useful on multirotors, allowing the craft yaw/heading to be correctly aligned to actual North simply by physically pointing the craft North then using the stick function to zero the yaw estimate. Since this also sets the heading as trusted Nav modes reliant on heading will be available immediately after arming without the need to fly fast enough to obtain a valid heading from GPS ground course.
+
 ## Yaw control
 
 While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the throttle is LOW (i.e. below the `min_check` setting).

src/main/fc/multifunction.c

@@ -70,6 +70,12 @@ static void multiFunctionApply(multi_function_e selectedItem)
         if (!ARMING_FLAG(ARMED)) {
             emergencyArmingUpdate(true, true);
         }
+        break;
+    case MULTI_FUNC_6:  // Calibrate compass/Zero yaw heading
+#if defined(USE_GPS) || defined(USE_MAG)
+        ENABLE_STATE(CALIBRATE_MAG);
+#endif
+        break;
     case MULTI_FUNC_END:
         break;
     }

src/main/fc/multifunction.h

@@ -54,6 +54,7 @@ typedef enum {
     MULTI_FUNC_3,
     MULTI_FUNC_4,
     MULTI_FUNC_5,
+    MULTI_FUNC_6,
     MULTI_FUNC_END,
 } multi_function_e;
 

src/main/flight/imu.c

@@ -56,6 +56,7 @@
 #endif
 
 #include "io/gps.h"
+#include "io/beeper.h"
 
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
@@ -85,7 +86,7 @@
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
 //centrifugal force compensated using gps
-FASTRAM fpVector3_t compansatedGravityBF;// cm/s/s
+FASTRAM fpVector3_t compensatedGravityBF;// cm/s/s
 
 STATIC_FASTRAM float smallAngleCosZ;
 
@@ -173,8 +174,7 @@ void imuConfigure(void)
      * 1000× per second; the kP gains only change when the user saves settings,
      * so computing it here (called once per save) avoids one add, one multiply,
      * and one divide on every PID cycle. */
-    imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f)
-        * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f;
+    imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f;
 }
 
 void imuInit(void)
@@ -192,7 +192,7 @@ void imuInit(void)
     // Create magnetic declination matrix
 #ifdef USE_MAG
     const int deg = compassConfig()->mag_declination / 100;
-    const int min = compassConfig()->mag_declination   % 100;
+    const int min = compassConfig()->mag_declination % 100;
 #else
     const int deg = 0;
     const int min = 0;
@@ -362,7 +362,7 @@ static float imuCalculateMcCogAccWeight(void)
 {
     fpVector3_t accBFNorm;
     vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS);
-    float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL
+    float wCoGAcc = constrainf((accBFNorm.z - 1.0f) * 2.0f, 0.0f, 1.0f); //z direction is verified via SITL
     wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); //reduce weight when fast angular rate, gps acc is considered lagging
     return wCoGAcc;
 }
@@ -437,7 +437,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             //vForward as trust vector
             if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
                 vForward.z = 1.0f;
-            }else{
+            } else {
                 vForward.x = 1.0f;
             }
             fpVector3_t vHeadingEF;
@@ -448,23 +448,22 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 float airSpeed = gpsSol.groundSpeed;
     #if defined(USE_WIND_ESTIMATOR)
                 // remove wind elements in vCoGlocal for better heading estimation
-                if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp)
-                {
+                if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) {
                     vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed);
                     vCoGlocal.x += getEstimatedWindSpeed(X);
                     vCoGlocal.y -= getEstimatedWindSpeed(Y);
                     airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal));
                 }
     #endif
-                wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
+                wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed) / 2.0f, 400.0f, 1000.0f), 400.0f, 1000.0f, 0.0f, 1.0f);
             } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero
                 wCoG = 0.0f;
             }
-            if (STATE(MULTIROTOR)){
+            if (STATE(MULTIROTOR)) {
                 //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
                 wCoG *= imuCalculateMcCogWeight();
                 //handle acc based vector
-                if(vCOGAcc){
+                if (vCOGAcc) {
                     float wCoGAcc = imuCalculateMcCogAccWeight();//stronger weight on acc if body frame z axis greate than 1G
                     if (wCoGAcc > wCoG){
                         //when copter is accelerating use gps acc vector instead of gps speed vector
@@ -627,8 +626,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 		attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0]));
 	}
 
-    if (attitude.values.yaw < 0)
-        attitude.values.yaw += 3600;
+    if (attitude.values.yaw < 0) attitude.values.yaw += 3600;
 
     /* Update small angle state */
     if (calculateCosTiltAngle() > smallAngleCosZ) {
@@ -667,23 +665,17 @@ static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_sl
     // Default - don't apply rate/ignore scaling
     float accWeight_RateIgnore = 1.0f;
 
-    if (ARMING_FLAG(ARMED) && imuConfig()->acc_ignore_rate)
-    {
+    if (ARMING_FLAG(ARMED) && imuConfig()->acc_ignore_rate) {
         float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
         rotRateMagnitude = rotRateMagnitude / (acc_ignore_slope_multipiler + 0.001f);
-        if (imuConfig()->acc_ignore_slope)
-        {
+
+        if (imuConfig()->acc_ignore_slope) {
             const float rateSlopeMin = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate - imuConfig()->acc_ignore_slope));
             const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope));
 
             accWeight_RateIgnore = scaleRangef(constrainf(rotRateMagnitude, rateSlopeMin, rateSlopeMax), rateSlopeMin, rateSlopeMax, 1.0f, 0.0f);
-        }
-        else
-        {
-            if (rotRateMagnitude > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate))
-            {
-                accWeight_RateIgnore = 0.0f;
-            }
+        } else if (rotRateMagnitude > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate)) {
+            accWeight_RateIgnore = 0.0f;
         }
     }
 
@@ -696,7 +688,7 @@ static void imuCalculateFilters(float dT)
     imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT);
     imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT);
     imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT);
-    
+
     imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT);
     imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT);
     imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT);
@@ -718,8 +710,7 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vE
 
     // on first gps data acquired, time_delta_ms will be large, vEstcentrifugalAccelBF will be minimal to disable the compensation
     rtcTime_t time_delta_ms = currenttime - lastGPSNewDataTime;
-    if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0)
-    {
+    if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) {
         // on new gps frame, update accEF and estimate centrifugal accleration
         vEstAccelEF->x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward
         vEstAccelEF->y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms));
@@ -734,11 +725,11 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vE
 }
 
 static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF, float dT, float *acc_ignore_slope_multipiler)
-{   
+{
     //fixed wing only
     static float lastspeed = -1.0f;
     float currentspeed = 0;
-    if (isGPSTrustworthy()){
+    if (isGPSTrustworthy()) {
         //first speed choice is gps
         static bool lastGPSHeartbeat;
         static pt1Filter_t GPS3DspeedFilter;
@@ -752,8 +743,7 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
         *acc_ignore_slope_multipiler = 4.0f;
     }
 #ifdef USE_PITOT
-    else if (sensors(SENSOR_PITOT) && pitotIsHealthy())
-    {
+    else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
         // second choice is pitot
 		currentspeed = getAirspeedEstimate();
         *acc_ignore_slope_multipiler = 2.0f;
@@ -776,7 +766,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){
     static bool firstRun = true;
     static fpQuaternion_t qNormal2Tail;
     static fpQuaternion_t qTail2Normal;
-    if(firstRun){
+    if (firstRun) {
         fpAxisAngle_t axisAngle;
         axisAngle.axis.x = 0;
         axisAngle.axis.y = 1;
@@ -792,7 +782,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){
 void imuUpdateTailSitter(void)
 {
     static bool lastTailSitter=false;
-    if (((bool)STATE(TAILSITTER)) != lastTailSitter){
+    if (((bool)STATE(TAILSITTER)) != lastTailSitter) {
         fpQuaternion_t* rotation_for_tailsitter= getTailSitterQuaternion(STATE(TAILSITTER));
         quaternionMultiply(&orientation, &orientation, rotation_for_tailsitter);
     }
@@ -842,65 +832,76 @@ static void imuCalculateEstimatedAttitude(float dT)
             resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
         }
     } else if (!ARMING_FLAG(ARMED)) {
-        gpsHeadingInitialized = false;
+        if (STATE(AIRPLANE)) {
+            gpsHeadingInitialized = false;  // required for fixed wing flight detection to work correctly
+        } else if (!sensors(SENSOR_MAG) && STATE(CALIBRATE_MAG)) {
+            // When no compass available allow yaw to be set to 0 (North) as required using compass calibration stick command
+            DISABLE_STATE(CALIBRATE_MAG);
+            beeper(BEEPER_ACTION_SUCCESS);
+
+            // Re-initialize quaternion from known Roll, Pitch with yaw set to 0 (North)
+            imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, 0);
+            gpsHeadingInitialized = true;
+        }
     }
 
     imuCalculateFilters(dT);
+
     // centrifugal force compensation
     static fpVector3_t vEstcentrifugalAccelBF_velned;
     static fpVector3_t vEstcentrifugalAccelBF_turnrate;
     float acc_ignore_slope_multipiler = 1.0f; // when using gps centrifugal_force_compensation, AccelerometerWeightRateIgnore slope will be multiplied by this value
-    if (isGPSTrustworthy())
-    {
+
+    if (isGPSTrustworthy()) {
         imuCalculateGPSacceleration(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler);
         useCOGAcc = true; //currently only for multicopter
     }
-    if (STATE(AIRPLANE))
-    {
+
+    if (STATE(AIRPLANE)) {
         imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler);
     }
-    if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE))
-    {
+
+    if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) {
         //pick the best centrifugal acceleration between velned and turnrate
-        fpVector3_t compansatedGravityBF_velned;
-        vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
-        float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
+        fpVector3_t compensatedGravityBF_velned;
+        vectorAdd(&compensatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
+        float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compensatedGravityBF_velned)) - GRAVITY_CMSS);
 
-        fpVector3_t compansatedGravityBF_turnrate;
-        vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
-        float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
+        fpVector3_t compensatedGravityBF_turnrate;
+        vectorAdd(&compensatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
+        float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compensatedGravityBF_turnrate)) - GRAVITY_CMSS);
 
-        compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned;
+        compensatedGravityBF = velned_error > turnrate_error ? compensatedGravityBF_turnrate : compensatedGravityBF_velned;
     }
     else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy())
     {
         //velned centrifugal force compensation, quad will use this method
-        vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
+        vectorAdd(&compensatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
     }
     else if (STATE(AIRPLANE))
     {
         //turnrate centrifugal force compensation
-        vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
+        vectorAdd(&compensatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
     }
     else
     {
-        compansatedGravityBF = imuMeasuredAccelBF;
+        compensatedGravityBF = imuMeasuredAccelBF;
     }
 #else
     // In absence of GPS MAG is the only option
     if (canUseMAG) {
         useMag = true;
     }
-    compansatedGravityBF = imuMeasuredAccelBF
+    compensatedGravityBF = imuMeasuredAccelBF;
 #endif
-    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
+    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compensatedGravityBF);
     accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
     const bool useAcc = (accWeight > 0.001f);
 
     const float magWeight = imuGetPGainScaleFactor() * 1.0f;
     fpVector3_t measuredMagBF = {.v = {mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
     imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF,
-                            useAcc ? &compansatedGravityBF : NULL,
+                            useAcc ? &compensatedGravityBF : NULL,
                             useMag ? &measuredMagBF : NULL,
                             useCOG ? &vCOG : NULL,
                             useCOGAcc ? &vCOGAcc : NULL,
@@ -950,7 +951,6 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         acc.accADCf[Z] = 0.0f;
     }
 }
- 
 
 bool isImuReady(void)
 {
@@ -966,9 +966,13 @@ float calculateCosTiltAngle(void)
 {
     return 1.0f - 2.0f * sq(orientation.q1) - 2.0f * sq(orientation.q2);
 }
-
+#if defined(USE_GPS)
+bool isYawZeroResetAllowed(void)
+{
+    return !ARMING_FLAG(ARMED) && !STATE(AIRPLANE) && sensors(SENSOR_GPS) && !sensors(SENSOR_MAG);
+}
+#endif
 #if defined(SITL_BUILD) || defined (USE_SIMULATOR)
-
 void imuSetAttitudeRPY(int16_t roll, int16_t pitch, int16_t yaw)
 {
     attitude.values.roll = roll;

src/main/flight/imu.h

@@ -25,10 +25,7 @@
 #include "config/parameter_group.h"
 
 extern fpVector3_t imuMeasuredAccelBF;         // cm/s/s
-extern fpVector3_t imuMeasuredAccelBFFiltered; // cm/s/s
 extern fpVector3_t imuMeasuredRotationBF;       // rad/s
-extern fpVector3_t imuMeasuredRotationBFFiltered;       // rad/s
-extern fpVector3_t compansatedGravityBF;         // cm/s/s
 extern fpVector3_t HeadVecEFFiltered;
 
 typedef union {
@@ -88,6 +85,7 @@ void imuUpdateAccelerometer(void);
 float calculateCosTiltAngle(void);
 bool isImuReady(void);
 bool isImuHeadingValid(void);
+bool isYawZeroResetAllowed(void);
 
 void imuTransformVectorBodyToEarth(fpVector3_t * v);
 void imuTransformVectorEarthToBody(fpVector3_t * v);

src/main/io/osd.c

@@ -6475,6 +6475,23 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
                 break;
             }
             activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_6:
+            if (!ARMING_FLAG(ARMED)) {
+#if defined(USE_MAG)
+                if (sensors(SENSOR_MAG)) {
+                    message = "CAL COMPAS";
+                    break;
+                }
+#endif
+#if defined(USE_GPS)
+                if (isYawZeroResetAllowed()) {
+                    message = "SET HEADIN";
+                    break;
+                }
+#endif
+            }
+            activeFunction++;
             break;
         case MULTI_FUNC_END:
             message = "*FUNC SET*";