[fusion] Enable fast heading convergence when mag cal happens

Fusion was tuned to take gyro readings in consideration more than mag
readings as gyro is usually more trustworthy and not susceptible to
various mag disturbances. This enables the fusion hold correct
headings for a while even if device is getting close/into to mag
disturbances caused by e.g. a magnet, steel structure or simply desk
with metal frame.

However, this causes issues when fusion is started in a "bad" region
where mag field is not pointing to earth mag north or when the mag
sensor is not calibrated.  The heading will be inevitably wrong in the
beginning. However, it is also desired to quickly converge to the
right heading after bad condition is lifted. If the user executed a
mag calibration, that means the user likely noticed problem in heading
and would like to correct. It is safe to assume the magnetic field
around is relatively clean as user is actively participating and
have the fusion quickly converge to current magnetic field reading.

Existing implementation cannot tell if the transition is towards the
good or bad, and thus being conservative at all time. This change
implements fast heading convergence at mag calibration, which will
greatly allievate aforementioned issue.

Bug: 30903098
Bug: 30130336
Bug: 30092812
Bug: 26496265

Change-Id: I51616c7614ec4657587df2b2720e10dd8f3f3d66
diff --git a/firmware/inc/algos/fusion.h b/firmware/inc/algos/fusion.h
index 506cd1c..6bf4878 100644
--- a/firmware/inc/algos/fusion.h
+++ b/firmware/inc/algos/fusion.h
@@ -22,6 +22,7 @@
 #include "mat.h"
 #include "quat.h"
 
+#include <stdbool.h>
 #include <stdint.h>
 #include <sys/types.h>
 
@@ -51,6 +52,9 @@
     uint32_t mCount[3];
     uint32_t flags;
 
+    float trustedMagDuration;
+    bool    lastMagInvalid;
+
     float  fake_mag_decimation;
     struct FusionParam param;
 };
@@ -61,17 +65,28 @@
     FUSION_REINITIALIZE = 1 << 2,
 };
 
+enum MagTrustMode {
+    NORMAL,
+    INITIALIZATION,  // right after initialization of fusion
+    BACK_TO_VALID,   // when the mag value goes from invalid to valid
+    MANUAL_MAG_CAL   // right after a manual calibration
+};
+
 void initFusion(struct Fusion *fusion, uint32_t flags);
 
 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT);
 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT);
-int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m);
+int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT);
+
+// set trust mode of mag sensors depending on scenarios, see MagTrustMode
+void fusionSetMagTrust(struct Fusion *fusion, int mode);
 
 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude);
 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias);
 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R);
 int fusionHasEstimate(const struct Fusion *fusion);
 
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/firmware/src/algos/fusion.c b/firmware/src/algos/fusion.c
index 537af51..a786cef 100644
--- a/firmware/src/algos/fusion.c
+++ b/firmware/src/algos/fusion.c
@@ -54,7 +54,7 @@
 #define MAX_VALID_MAGNETIC_FIELD    75.0f
 #define MAX_VALID_MAGNETIC_FIELD_SQ (MAX_VALID_MAGNETIC_FIELD * MAX_VALID_MAGNETIC_FIELD)
 
-#define MIN_VALID_MAGNETIC_FIELD    30.0f
+#define MIN_VALID_MAGNETIC_FIELD    20.0f   //norminal mag field strength is 25uT in some area
 #define MIN_VALID_MAGNETIC_FIELD_SQ (MIN_VALID_MAGNETIC_FIELD * MIN_VALID_MAGNETIC_FIELD)
 
 #define MIN_VALID_CROSS_PRODUCT_MAG     1.0e-3
@@ -62,6 +62,8 @@
 
 #define DELTA_TIME_MARGIN 1.0e-9f
 
+#define TRUST_DURATION_MANUAL_MAG_CAL      5.f  //unit: seconds
+
 void initFusion(struct Fusion *fusion, uint32_t flags) {
     fusion->flags = flags;
 
@@ -95,12 +97,16 @@
         initVec3(&fusion->mData[0], 0.0f, 0.0f, 0.0f);
         initVec3(&fusion->mData[1], 0.0f, 0.0f, 0.0f);
         initVec3(&fusion->mData[2], 0.0f, 0.0f, 0.0f);
+
     } else  {
         // mask off disabled sensor bit
         fusion->mInitState &= (ACC
                                | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0)
                                | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0));
     }
+
+    fusionSetMagTrust(fusion, NORMAL);
+    fusion->lastMagInvalid = false;
 }
 
 int fusionHasEstimate(const struct Fusion *fusion) {
@@ -218,6 +224,8 @@
         initZeroMatrix(&fusion->P[0][1]);
         initZeroMatrix(&fusion->P[1][0]);
         initZeroMatrix(&fusion->P[1][1]);
+
+        fusionSetMagTrust(fusion, INITIALIZATION);
     }
 
     return 0;
@@ -576,10 +584,11 @@
     return 0;
 }
 
-#define MAG_COS_CONV_FACTOR  0.02f
-#define MAG_COS_CONV_LIMIT    2.f
+#define MAG_COS_CONV_FACTOR   0.02f
+#define MAG_COS_CONV_LIMIT    3.5f
+#define MAG_STDEV_REDUCTION   0.005f // lower stdev means more trust
 
-int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m) {
+int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT) {
     if (!fusion_init_complete(fusion, MAG, m, 0.0f /* dT */)) {
         return -EINVAL;
     }
@@ -588,6 +597,8 @@
 
     if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ
             || magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
+        fusionSetMagTrust(fusion, NORMAL);
+        fusion->lastMagInvalid = true;
         return -EINVAL;
     }
 
@@ -601,9 +612,16 @@
     vec3Cross(&east, m, &up);
 
     if (vec3NormSquared(&east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
+        fusionSetMagTrust(fusion, NORMAL);
+        fusion->lastMagInvalid = true;
         return -EINVAL;
     }
 
+    if (fusion->lastMagInvalid) {
+        fusion->lastMagInvalid = false;
+        fusionSetMagTrust(fusion, BACK_TO_VALID);
+    }
+
     struct Vec3 north;
     vec3Cross(&north, &up, &east);
 
@@ -616,12 +634,26 @@
         struct Vec3 mm;
         mat33Apply(&mm, &R, &fusion->Bm);
         float cos_err = vec3Dot(&mm, &north);
-        cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ?
-            (1.f - MAG_COS_CONV_FACTOR) : cos_err;
 
-        float fc;
-        fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT);
-        p *= expf(-fc);
+        if (fusion->trustedMagDuration > 0) {
+            // if the trust mag time period is not finished
+            if (cos_err < (1.f - MAG_COS_CONV_FACTOR/4)) {
+                // if the mag direction and the fusion north has not converged, lower the
+                // standard deviation of mag to speed up convergence.
+                p *= MAG_STDEV_REDUCTION;
+                fusion->trustedMagDuration -= dT;
+            } else {
+                // it has converged already, so no need to keep the trust period any longer
+                fusionSetMagTrust(fusion, NORMAL);
+            }
+        } else {
+            cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ?
+                (1.f - MAG_COS_CONV_FACTOR) : cos_err;
+
+            float fc;
+            fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT);
+            p *= expf(-fc);
+        }
     }
 
     fusionUpdate(fusion, &north, &fusion->Bm, p);
@@ -629,6 +661,24 @@
     return 0;
 }
 
+void fusionSetMagTrust(struct Fusion *fusion, int mode) {
+    switch(mode) {
+        case NORMAL:
+            fusion->trustedMagDuration = 0; // disable
+            break;
+        case INITIALIZATION:
+        case BACK_TO_VALID:
+            fusion->trustedMagDuration = 0; // no special treatment for these two
+            break;
+        case MANUAL_MAG_CAL:
+            fusion->trustedMagDuration = TRUST_DURATION_MANUAL_MAG_CAL;
+            break;
+        default:
+            fusion->trustedMagDuration = 0; // by default it is disable
+            break;
+    }
+}
+
 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) {
     *attitude = fusion->x0;
 }
diff --git a/firmware/src/drivers/orientation/orientation.c b/firmware/src/drivers/orientation/orientation.c
index 7bb06ef..c9bd5e2 100644
--- a/firmware/src/drivers/orientation/orientation.c
+++ b/firmware/src/drivers/orientation/orientation.c
@@ -29,6 +29,7 @@
 #include <nanohub_math.h>
 #include <algos/fusion.h>
 #include <sensors.h>
+#include <variant/inc/sensType.h>
 #include <limits.h>
 #include <slab.h>
 
@@ -51,6 +52,7 @@
 #define EVT_SENSOR_ACC_DATA_RDY     sensorGetMyEventType(SENS_TYPE_ACCEL)
 #define EVT_SENSOR_GYR_DATA_RDY     sensorGetMyEventType(SENS_TYPE_GYRO)
 #define EVT_SENSOR_MAG_DATA_RDY     sensorGetMyEventType(SENS_TYPE_MAG)
+#define EVT_SENSOR_MAG_BIAS         sensorGetMyEventType(SENS_TYPE_MAG_BIAS)
 
 #define kGravityEarth               9.80665f
 #define kRad2deg                    (180.0f / M_PI)
@@ -478,7 +480,7 @@
         case MAG:
             initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z);
 
-            fusionHandleMag(&mTask.fusion, &m);
+            fusionHandleMag(&mTask.fusion, &m, dT);
 
             --mTask.sample_counts[MAG];
             if (++k == MAX_NUM_SAMPLES)
@@ -589,6 +591,7 @@
             if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
                         mTask.raw_sensor_latency)) {
                 osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
+                osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS);
                 break;
             }
         }
@@ -760,6 +763,13 @@
         fillSamples(ev, GYR);
         drainSamples();
         break;
+    case EVT_SENSOR_MAG_BIAS:
+        ev = (struct TripleAxisDataEvent *)evtData;
+        if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) {
+            //it is a user initiated mag cal event
+            fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL);
+        }
+        break;
     case EVT_SENSOR_MAG_DATA_RDY:
         ev = (struct TripleAxisDataEvent *)evtData;
         fillSamples(ev, MAG);