From e17ae02b347799db85c1429d1ad098fe243c99b7 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Sun, 22 Oct 2023 14:21:46 +0900
Subject: [PATCH 01/22] ahrs mag gps fusion

---
 src/main/flight/imu.c | 65 ++++++++++++++++++++++++++++++++-----------
 1 file changed, 48 insertions(+), 17 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index ed5cbcbfaa4..a7b61e4819a 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -77,6 +77,7 @@
 
 #define SPIN_RATE_LIMIT             20
 #define MAX_ACC_NEARNESS            0.2    // 20% or G error soft-accepted (0.8-1.2G)
+#define MAX_MAG_NEARNESS            0.2    // 20% or magnetic field error soft-accepted (0.8-1.2)
 #define IMU_ROTATION_LPF         3       // Hz
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
@@ -111,6 +112,8 @@ FASTRAM bool gpsHeadingInitialized;
 
 FASTRAM bool imuUpdated = false;
 
+static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF);
+
 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
@@ -326,6 +329,15 @@ bool isGPSTrustworthy(void)
     return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
 }
 
+static float imuCalculateMcMagCogWeight(void)
+{
+    float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
+    float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
+    const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope)) * 4.0f;
+    wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
+    return wCoG;
+}
+
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
@@ -339,11 +351,13 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
     /* Step 1: Yaw correction */
     // Use measured magnetic field vector
     if (magBF || useCOG) {
-        static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
-
-        fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        float wMag;
+        float wCoG;
+        fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 
         if (magBF && vectorNormSquared(magBF) > 0.01f) {
+            wMag = bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct
             fpVector3_t vMag;
 
             // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
@@ -369,13 +383,26 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
                 // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
                 // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
-                vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
+                vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth);
 
                 // Rotate error back into body frame
-                quaternionRotateVector(&vErr, &vErr, &orientation);
+                quaternionRotateVector(&vMagErr, &vMagErr, &orientation);
             }
         }
-        else if (useCOG) {
+        if (useCOG) {
+            fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
+            //vForward as trust vector
+            if STATE(MULTIROTOR){
+                vForward.z = 1.0f;
+            }else{
+                vForward.x = 1.0f;
+            }
+
+            wCoG = scaleRangef(constrainf(gpsSol.groundSpeed, 300, 1200), 300, 1200, 0.0f, 1.0f);
+            if (STATE(MULTIROTOR)){
+                //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
+                wCoG *= imuCalculateMcMagCogWeight();
+            }
             fpVector3_t vHeadingEF;
 
             // Use raw heading error (from GPS or whatever else)
@@ -409,13 +436,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorNormalize(&vHeadingEF, &vHeadingEF);
 
                 // error is cross product between reference heading and estimated heading (calculated in EF)
-                vectorCrossProduct(&vErr, &vCoG, &vHeadingEF);
+                vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF);
 
                 // Rotate error back into body frame
-                quaternionRotateVector(&vErr, &vErr, &orientation);
+                quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation);
             }
         }
-
+        fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        vectorScale(&vMagErr, &vMagErr, wMag);
+        vectorScale(&vCoGErr, &vCoGErr, wCoG);
+        vectorAdd(&vErr, &vMagErr, &vCoGErr);
         // Compute and apply integral feedback if enabled
         if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
             // Stop integrating if spinning beyond the certain limit
@@ -535,10 +565,10 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
     }
 }
 
-static float imuCalculateAccelerometerWeightNearness(void)
+static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF)
 {
     fpVector3_t accBFNorm;
-    vectorScale(&accBFNorm, &compansatedGravityBF, 1.0f / GRAVITY_CMSS);
+    vectorScale(&accBFNorm, accBF, 1.0f / GRAVITY_CMSS);
     const float accMagnitudeSq = vectorNormSquared(&accBFNorm);
     const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
     return accWeight_Nearness;
@@ -672,20 +702,21 @@ static void imuCalculateEstimatedAttitude(float dT)
     bool useMag = false;
     bool useCOG = false;
 #if defined(USE_GPS)
-    if (STATE(FIXED_WING_LEGACY)) {
+    if (STATE(FIXED_WING_LEGACY) || STATE(MULTIROTOR)) {
         bool canUseCOG = isGPSHeadingValid();
 
-        // Prefer compass (if available)
+        // Use compass (if available)
         if (canUseMAG) {
             useMag = true;
             gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
         }
-        else if (canUseCOG) {
+        // Use GPS (if available)
+        if (canUseCOG) {
             if (gpsHeadingInitialized) {
                 courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
                 useCOG = true;
             }
-            else {
+            else if (!canUseMAG) {
                 // Re-initialize quaternion from known Roll, Pitch and GPS heading
                 imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
                 gpsHeadingInitialized = true;
@@ -698,7 +729,7 @@ static void imuCalculateEstimatedAttitude(float dT)
         }
     }
     else {
-        // Multicopters don't use GPS heading
+        // other platform type don't use GPS heading
         if (canUseMAG) {
             useMag = true;
         }
@@ -751,7 +782,7 @@ static void imuCalculateEstimatedAttitude(float dT)
     }
     compansatedGravityBF = imuMeasuredAccelBF
 #endif
-    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness();
+    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
     accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
     const bool useAcc = (accWeight > 0.001f);
 

From 69ac08048e83b3e15640e417b312d9288d8767d9 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Sun, 22 Oct 2023 14:29:07 +0900
Subject: [PATCH 02/22] minor fix

---
 src/main/flight/imu.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index a7b61e4819a..e93aac430bb 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -351,8 +351,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
     /* Step 1: Yaw correction */
     // Use measured magnetic field vector
     if (magBF || useCOG) {
-        float wMag;
-        float wCoG;
+        float wMag = 0.0f;
+        float wCoG = 0.0f;
         fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
         fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 

From 8e3ee38e518b51a70714e77fa6e8d1aa8e2cd269 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Sun, 22 Oct 2023 16:51:09 +0900
Subject: [PATCH 03/22] development of posestimator fusion

---
 src/main/fc/settings.yaml                      |  4 ++--
 src/main/navigation/navigation_pos_estimator.c | 16 +++++++++-------
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 87daf753cf2..c719c4003f0 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2314,13 +2314,13 @@ groups:
         max: 100
         default_value: 2.0
       - name: inav_w_z_baro_p
-        description: "Weight of barometer measurements in estimated altitude and climb rate"
+        description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors."
         field: w_z_baro_p
         min: 0
         max: 10
         default_value: 0.35
       - name: inav_w_z_gps_p
-        description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes"
+        description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p
         min: 0
         max: 10
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 5c314bfddda..d83836bfb14 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -570,7 +570,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
-    if (ctx->newFlags & EST_BARO_VALID) {
+    bool correctOK = false;
+    //use both baro and gps
+
+    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro)) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -589,7 +592,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             }
         }
 
-        // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
+        // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
         bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
                                             (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
                                              ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
@@ -614,10 +617,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
-        return true;
+        correctOK = true;
     }
-    else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
-        // If baro is not available - use GPS Z for correction on a plane
+    if (ctx->newFlags & EST_GPS_Z_VALID) {
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -637,10 +639,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
         }
 
-        return true;
+        correctOK = true;
     }
 
-    return false;
+    return correctOK;
 }
 
 static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)

From 00b46bc0d6e5828e55b4e6df6592beec5b267e5d Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Sun, 22 Oct 2023 18:16:19 +0900
Subject: [PATCH 04/22] development of posestimator fusion

---
 src/main/fc/settings.yaml                      |  4 ++--
 src/main/navigation/navigation_pos_estimator.c | 16 +++++-----------
 2 files changed, 7 insertions(+), 13 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c719c4003f0..254ad509ec5 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2324,13 +2324,13 @@ groups:
         field: w_z_gps_p
         min: 0
         max: 10
-        default_value: 0.2
+        default_value: 0.5
       - name: inav_w_z_gps_v
         description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
         field: w_z_gps_v
         min: 0
         max: 10
-        default_value: 0.1
+        default_value: 0.5
       - name: inav_w_xy_gps_p
         description: "Weight of GPS coordinates in estimated UAV position and speed."
         default_value: 1.0
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index d83836bfb14..fa0aa5db0c2 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -571,9 +571,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     bool correctOK = false;
+    
+    const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro is deference is too big
     //use both baro and gps
-
-    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro)) {
+    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv)) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -602,14 +603,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
         ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
 
-        // If GPS is available - also use GPS climb rate
-        if (ctx->newFlags & EST_GPS_Z_VALID) {
-            // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
-            const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-            const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
-            ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
-        }
-
         ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 
         // Accelerometer bias
@@ -629,10 +622,11 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         else {
             // Altitude
             const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
+            const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
 
             ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
             ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
-            ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
+            ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
             ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
 
             // Accelerometer bias

From a4c959943d3b59f3db138e1527674ff3e3ae537c Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Mon, 23 Oct 2023 02:12:30 +0900
Subject: [PATCH 05/22] development of posestimator fusion

---
 src/main/fc/settings.yaml                      | 2 +-
 src/main/navigation/navigation_pos_estimator.c | 8 +++-----
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 254ad509ec5..9d31a1e231d 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2330,7 +2330,7 @@ groups:
         field: w_z_gps_v
         min: 0
         max: 10
-        default_value: 0.5
+        default_value: 1.0
       - name: inav_w_xy_gps_p
         description: "Weight of GPS coordinates in estimated UAV position and speed."
         default_value: 1.0
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index fa0aa5db0c2..f8473a354f9 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -572,9 +572,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
     bool correctOK = false;
     
-    const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro is deference is too big
+    const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro if difference is too big, baro is probably wrong
     //use both baro and gps
-    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv)) {
+    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -584,9 +584,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
         else {
             if (posEstimator.est.vel.z > 15) {
-                if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
-                    posEstimator.state.isBaroGroundValid = false;
-                }
+                posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
             }
             else {
                 posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec

From 04a9d8cbb8fa230e50dde13837b1be10baadf7e5 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Tue, 24 Oct 2023 10:38:01 +0900
Subject: [PATCH 06/22] modify the weights

---
 src/main/flight/imu.c | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index e93aac430bb..08373b6e5d0 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -351,13 +351,13 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
     /* Step 1: Yaw correction */
     // Use measured magnetic field vector
     if (magBF || useCOG) {
-        float wMag = 0.0f;
-        float wCoG = 0.0f;
+        float wMag = 1.0f;
+        float wCoG = 1.0f;
         fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
         fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 
         if (magBF && vectorNormSquared(magBF) > 0.01f) {
-            wMag = bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct
+            wMag *= bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct
             fpVector3_t vMag;
 
             // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
@@ -397,11 +397,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             }else{
                 vForward.x = 1.0f;
             }
-
-            wCoG = scaleRangef(constrainf(gpsSol.groundSpeed, 300, 1200), 300, 1200, 0.0f, 1.0f);
             if (STATE(MULTIROTOR)){
                 //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
                 wCoG *= imuCalculateMcMagCogWeight();
+                //scale accroading to multirotor`s tilt angle
+                wCoG *= scaleRangef(constrainf(vForward.z, 0.98f, 0.90f), 0.98f, 0.90f, 0.0f, 1.0f); //cos(10deg) and cos(25deg)
             }
             fpVector3_t vHeadingEF;
 
@@ -413,6 +413,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             // (Rxx; Ryx) - measured (estimated) heading vector (EF)
             // (-cos(COG), sin(COG)) - reference heading vector (EF)
 
+            float airSpeed = gpsSol.groundSpeed;
             // Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards.
             fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
 #if defined(USE_WIND_ESTIMATOR)
@@ -422,10 +423,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed);
                 vCoG.x += getEstimatedWindSpeed(X);
                 vCoG.y -= getEstimatedWindSpeed(Y);
+                airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG));
                 vectorNormalize(&vCoG, &vCoG);
             }
 #endif
-
+            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 300, 1200), 300, 1200, 0.0f, 1.0f);
             // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
             quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
             vHeadingEF.z = 0.0f;

From c1c4b8360a9d8fa582265cb4a0126766ad85064e Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Tue, 24 Oct 2023 21:04:01 +0900
Subject: [PATCH 07/22] update the imu heading estimation

---
 src/main/fc/settings.yaml |  4 ++--
 src/main/flight/imu.c     | 11 +++++++----
 src/main/io/gps.c         |  2 +-
 3 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 9d31a1e231d..fe950eca744 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2324,13 +2324,13 @@ groups:
         field: w_z_gps_p
         min: 0
         max: 10
-        default_value: 0.5
+        default_value: 0.4
       - name: inav_w_z_gps_v
         description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
         field: w_z_gps_v
         min: 0
         max: 10
-        default_value: 1.0
+        default_value: 0.8
       - name: inav_w_xy_gps_p
         description: "Weight of GPS coordinates in estimated UAV position and speed."
         default_value: 1.0
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 08373b6e5d0..4f55bad7f4b 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -337,7 +337,8 @@ static float imuCalculateMcMagCogWeight(void)
     wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
     return wCoG;
 }
-
+#define COS10DEG 0.984f
+#define COS25DEG 0.906f
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
@@ -357,7 +358,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 
         if (magBF && vectorNormSquared(magBF) > 0.01f) {
-            wMag *= bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct
+            wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS);
             fpVector3_t vMag;
 
             // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
@@ -401,7 +402,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
                 wCoG *= imuCalculateMcMagCogWeight();
                 //scale accroading to multirotor`s tilt angle
-                wCoG *= scaleRangef(constrainf(vForward.z, 0.98f, 0.90f), 0.98f, 0.90f, 0.0f, 1.0f); //cos(10deg) and cos(25deg)
+                wCoG *= scaleRangef(constrainf(vForward.z, COS10DEG, COS25DEG), COS10DEG, COS25DEG, 0.0f, 1.0f);
+                //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight
             }
             fpVector3_t vHeadingEF;
 
@@ -839,6 +841,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         acc.accADCf[Z] = 0.0f;
     }
 }
+ 
 
 bool isImuReady(void)
 {
@@ -847,7 +850,7 @@ bool isImuReady(void)
 
 bool isImuHeadingValid(void)
 {
-    return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
+    return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized;
 }
 
 float calculateCosTiltAngle(void)
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 0fae3a7f318..87999e594a0 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -453,7 +453,7 @@ bool isGPSHealthy(void)
 
 bool isGPSHeadingValid(void)
 {
-    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
+    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 500;
 }
 
 #endif

From 1c5a42cf4e0968faf4291b6ceb904e6ae8b175aa Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Tue, 24 Oct 2023 22:45:11 +0900
Subject: [PATCH 08/22] gps baro mag mix

---
 src/main/drivers/compass/compass_fake.c |  2 +-
 src/main/fc/settings.yaml               |  5 +++
 src/main/flight/imu.c                   | 55 +++++++++++--------------
 src/main/flight/imu.h                   |  1 +
 src/main/io/gps.c                       |  2 +-
 src/main/target/SITL/sim/realFlight.c   |  6 +--
 src/main/target/SITL/sim/xplane.c       |  6 +--
 7 files changed, 38 insertions(+), 39 deletions(-)

diff --git a/src/main/drivers/compass/compass_fake.c b/src/main/drivers/compass/compass_fake.c
index a95b3297a5f..2ef8692fd2d 100644
--- a/src/main/drivers/compass/compass_fake.c
+++ b/src/main/drivers/compass/compass_fake.c
@@ -37,7 +37,7 @@ static bool fakeMagInit(magDev_t *magDev)
 {
     UNUSED(magDev);
     // initially point north
-    fakeMagData[X] = 4096;
+    fakeMagData[X] = 1024;
     fakeMagData[Y] = 0;
     fakeMagData[Z] = 0;
     return true;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index fe950eca744..55ebd811b0f 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1455,6 +1455,11 @@ groups:
         default_value: ADAPTIVE
         field: inertia_comp_method
         table: imu_inertia_comp_method
+      - name: ahrs_use_mag_no_gps
+        description: "Arhs will only use mag when gps is still avaliable"
+        default_value: OFF
+        field: use_mag_no_gps
+        type: bool
 
   - name: PG_ARMING_CONFIG
     type: armingConfig_t
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 4f55bad7f4b..9425218707f 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -125,7 +125,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
     .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
     .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
-    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
+    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT,
+    .use_mag_no_gps = SETTING_AHRS_USE_MAG_NO_GPS_DEFAULT
 );
 
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@@ -337,7 +338,7 @@ static float imuCalculateMcMagCogWeight(void)
     wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
     return wCoG;
 }
-#define COS10DEG 0.984f
+#define COS5DEG 0.996f
 #define COS25DEG 0.906f
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
 {
@@ -402,7 +403,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
                 wCoG *= imuCalculateMcMagCogWeight();
                 //scale accroading to multirotor`s tilt angle
-                wCoG *= scaleRangef(constrainf(vForward.z, COS10DEG, COS25DEG), COS10DEG, COS25DEG, 0.0f, 1.0f);
+                wCoG *= scaleRangef(constrainf(vForward.z, COS5DEG, COS25DEG), COS5DEG, COS25DEG, 0.0f, 1.0f);
                 //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight
             }
             fpVector3_t vHeadingEF;
@@ -429,7 +430,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorNormalize(&vCoG, &vCoG);
             }
 #endif
-            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 300, 1200), 300, 1200, 0.0f, 1.0f);
+            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1500), 400, 1500, 0.0f, 1.0f);
             // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
             quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
             vHeadingEF.z = 0.0f;
@@ -706,37 +707,29 @@ static void imuCalculateEstimatedAttitude(float dT)
     bool useMag = false;
     bool useCOG = false;
 #if defined(USE_GPS)
-    if (STATE(FIXED_WING_LEGACY) || STATE(MULTIROTOR)) {
-        bool canUseCOG = isGPSHeadingValid();
+    bool canUseCOG = isGPSHeadingValid();
 
-        // Use compass (if available)
-        if (canUseMAG) {
-            useMag = true;
-            gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
+    // Use compass (if available)
+    if (canUseMAG) {
+        useMag = true;
+        gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
+    }
+    // Use GPS (if available)
+    if (canUseCOG && (!(imuConfig()->use_mag_no_gps && useMag))) {
+        if (gpsHeadingInitialized) {
+            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+            useCOG = true;
         }
-        // Use GPS (if available)
-        if (canUseCOG) {
-            if (gpsHeadingInitialized) {
-                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-                useCOG = true;
-            }
-            else if (!canUseMAG) {
-                // Re-initialize quaternion from known Roll, Pitch and GPS heading
-                imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
-                gpsHeadingInitialized = true;
+        else if (!canUseMAG) {
+            // Re-initialize quaternion from known Roll, Pitch and GPS heading
+            imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
+            gpsHeadingInitialized = true;
 
-                // Force reset of heading hold target
-                resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
-            }
-        } else if (!ARMING_FLAG(ARMED)) {
-            gpsHeadingInitialized = false;
-        }
-    }
-    else {
-        // other platform type don't use GPS heading
-        if (canUseMAG) {
-            useMag = true;
+            // Force reset of heading hold target
+            resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
         }
+    } else if (!ARMING_FLAG(ARMED)) {
+        gpsHeadingInitialized = false;
     }
 
     imuCalculateFilters(dT);
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index a3b87e67360..24904b98ba6 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -54,6 +54,7 @@ typedef struct imuConfig_s {
     uint8_t acc_ignore_slope;
     uint8_t gps_yaw_windcomp;
     uint8_t inertia_comp_method;
+    uint8_t use_mag_no_gps;
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 87999e594a0..f71abe897d4 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -453,7 +453,7 @@ bool isGPSHealthy(void)
 
 bool isGPSHeadingValid(void)
 {
-    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 500;
+    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 400;
 }
 
 #endif
diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index 23e77bcde04..05ee701ddd8 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -399,9 +399,9 @@ static void exchangeData(void)
     computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
     transformVectorEarthToBody(&north, &quat);
     fakeMagSet(
-        constrainToInt16(north.x * 16000.0f),
-        constrainToInt16(north.y * 16000.0f),
-        constrainToInt16(north.z * 16000.0f)
+        constrainToInt16(north.x * 1024.0f),
+        constrainToInt16(north.y * 1024.0f),
+        constrainToInt16(north.z * 1024.0f)
     );
 
     free(rfValues.m_currentAircraftStatus);
diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c
index 44089117d2e..d629c2130e3 100644
--- a/src/main/target/SITL/sim/xplane.c
+++ b/src/main/target/SITL/sim/xplane.c
@@ -421,9 +421,9 @@ static void* listenWorker(void* arg)
         computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
         transformVectorEarthToBody(&north, &quat);
         fakeMagSet(
-            constrainToInt16(north.x * 16000.0f),
-            constrainToInt16(north.y * 16000.0f),
-            constrainToInt16(north.z * 16000.0f)
+            constrainToInt16(north.x * 1024.0f),
+            constrainToInt16(north.y * 1024.0f),
+            constrainToInt16(north.z * 1024.0f)
         );
 
         if (!initalized) {

From cd0206a48f6a278f141e89f922e3e182c55518d3 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Tue, 24 Oct 2023 22:48:46 +0900
Subject: [PATCH 09/22] gps baro mag mix

---
 src/main/fc/settings.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 55ebd811b0f..237cf99cfdd 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2323,7 +2323,7 @@ groups:
         field: w_z_baro_p
         min: 0
         max: 10
-        default_value: 0.35
+        default_value: 0.4
       - name: inav_w_z_gps_p
         description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p
@@ -2335,7 +2335,7 @@ groups:
         field: w_z_gps_v
         min: 0
         max: 10
-        default_value: 0.8
+        default_value: 0.6
       - name: inav_w_xy_gps_p
         description: "Weight of GPS coordinates in estimated UAV position and speed."
         default_value: 1.0

From 4af73a7c85648eb3ba7b14856e11cebfd4dcde32 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 02:52:49 +0900
Subject: [PATCH 10/22] bug fixes on mag gps mix

---
 src/main/flight/imu.c                 | 28 ++++++++++++++-------------
 src/main/io/gps_fake.c                |  1 +
 src/main/target/SITL/sim/realFlight.c | 16 +++++++--------
 3 files changed, 24 insertions(+), 21 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 9425218707f..3d0ce791f2a 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -77,7 +77,9 @@
 
 #define SPIN_RATE_LIMIT             20
 #define MAX_ACC_NEARNESS            0.2    // 20% or G error soft-accepted (0.8-1.2G)
-#define MAX_MAG_NEARNESS            0.2    // 20% or magnetic field error soft-accepted (0.8-1.2)
+#define MAX_MAG_NEARNESS            0.25    // 25% or magnetic field error soft-accepted (0.75-1.25)
+#define COS5DEG 0.996f
+#define COS20DEG 0.940f
 #define IMU_ROTATION_LPF         3       // Hz
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
@@ -330,7 +332,7 @@ bool isGPSTrustworthy(void)
     return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
 }
 
-static float imuCalculateMcMagCogWeight(void)
+static float imuCalculateMcCogWeight(void)
 {
     float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
     float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
@@ -338,8 +340,7 @@ static float imuCalculateMcMagCogWeight(void)
     wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
     return wCoG;
 }
-#define COS5DEG 0.996f
-#define COS25DEG 0.906f
+
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
@@ -394,18 +395,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         if (useCOG) {
             fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
             //vForward as trust vector
-            if STATE(MULTIROTOR){
+            if (STATE(MULTIROTOR)){
                 vForward.z = 1.0f;
             }else{
                 vForward.x = 1.0f;
             }
-            if (STATE(MULTIROTOR)){
-                //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
-                wCoG *= imuCalculateMcMagCogWeight();
-                //scale accroading to multirotor`s tilt angle
-                wCoG *= scaleRangef(constrainf(vForward.z, COS5DEG, COS25DEG), COS5DEG, COS25DEG, 0.0f, 1.0f);
-                //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight
-            }
             fpVector3_t vHeadingEF;
 
             // Use raw heading error (from GPS or whatever else)
@@ -430,9 +424,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorNormalize(&vCoG, &vCoG);
             }
 #endif
-            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1500), 400, 1500, 0.0f, 1.0f);
+            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1200), 400, 1200, 0.0f, 1.0f);
             // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
             quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
+
+            if (STATE(MULTIROTOR)){
+                //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
+                wCoG *= imuCalculateMcCogWeight();
+                //scale accroading to multirotor`s tilt angle
+                wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS5DEG), COS20DEG, COS5DEG, 1.0f, 0.0f);
+                //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed
+            }
             vHeadingEF.z = 0.0f;
 
             // We zeroed out vHeadingEF.z -  make sure the whole vector didn't go to zero
diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c
index 6d574893436..5b0de8c50b5 100644
--- a/src/main/io/gps_fake.c
+++ b/src/main/io/gps_fake.c
@@ -31,6 +31,7 @@
 
 #include "platform.h"
 #include "build/build_config.h"
+#include "common/log.h"
 
 #if defined(USE_GPS_FAKE)
 
diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index 05ee701ddd8..b4e5b9283ab 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -296,9 +296,9 @@ static void exchangeData(void)
     //rfValues.m_orientationQuaternion_W = getDoubleFromResponse(response, "m-orientationQuaternion-W");
     rfValues.m_aircraftPositionX_MTR = getDoubleFromResponse(response, "m-aircraftPositionX-MTR");
     rfValues.m_aircraftPositionY_MTR = getDoubleFromResponse(response, "m-aircraftPositionY-MTR");
-    //rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
-    //rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
-    //rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
+    rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
+    rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
+    rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
     //rfValues.m_velocityBodyU_MPS = getDoubleFromResponse(response, "m-velocityBodyU-MPS");
     //rfValues.m_velocityBodyV_MPS = getDoubleFromResponse(response, "mm-velocityBodyV-MPS");
     //rfValues.m_velocityBodyW_MPS = getDoubleFromResponse(response, "m-velocityBodyW-MPS");
@@ -332,7 +332,7 @@ static void exchangeData(void)
     float lat, lon;
     fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon);
     
-    int16_t course = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
+    int16_t course = (int16_t)roundf(RADIANS_TO_DECIDEGREES(atan2_approx(-rfValues.m_velocityWorldU_MPS,rfValues.m_velocityWorldV_MPS)));
     int32_t altitude = (int32_t)roundf(rfValues.m_altitudeASL_MTR * 100);
     gpsFakeSet(
         GPS_FIX_3D,
@@ -342,9 +342,9 @@ static void exchangeData(void)
         altitude,
         (int16_t)roundf(rfValues.m_groundspeed_MPS * 100),
         course,
-        0, 
-        0,
-        0,
+        0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction
+        0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),
+        0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),
         0
     );
 
@@ -357,7 +357,7 @@ static void exchangeData(void)
 
     const int16_t roll_inav = (int16_t)roundf(rfValues.m_roll_DEG * 10);
     const int16_t pitch_inav = (int16_t)roundf(-rfValues.m_inclination_DEG * 10);
-    const int16_t yaw_inav = course;
+    const int16_t yaw_inav = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
     if (!useImu) {
         imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
         imuUpdateAttitude(micros());

From 4f518e7f5601d9572f584dff58cf1081fa9b234e Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 03:11:24 +0900
Subject: [PATCH 11/22] minor adjuestments

---
 src/main/flight/imu.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 3d0ce791f2a..2b13880d0ff 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -336,7 +336,7 @@ static float imuCalculateMcCogWeight(void)
 {
     float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
     float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
-    const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope)) * 4.0f;
+    const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f;
     wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
     return wCoG;
 }
@@ -424,7 +424,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorNormalize(&vCoG, &vCoG);
             }
 #endif
-            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1200), 400, 1200, 0.0f, 1.0f);
+            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
             // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
             quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
 

From 9dbbd25f70ffdcd1d3789fc1f22859e8015013b3 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 03:36:18 +0900
Subject: [PATCH 12/22] adjuestable gps yaw weight

---
 src/main/fc/settings.yaml | 11 ++++++-----
 src/main/flight/imu.c     |  6 ++++--
 src/main/flight/imu.h     |  2 +-
 3 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 237cf99cfdd..5d21793c0d3 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1455,11 +1455,12 @@ groups:
         default_value: ADAPTIVE
         field: inertia_comp_method
         table: imu_inertia_comp_method
-      - name: ahrs_use_mag_no_gps
-        description: "Arhs will only use mag when gps is still avaliable"
-        default_value: OFF
-        field: use_mag_no_gps
-        type: bool
+      - name: ahrs_gps_yaw_weight
+        description: "Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass"
+        default_value: 100
+        field: gps_yaw_weight
+        min: 0
+        max: 500
 
   - name: PG_ARMING_CONFIG
     type: armingConfig_t
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 2b13880d0ff..a5eba967501 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -128,7 +128,7 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
     .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
     .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT,
-    .use_mag_no_gps = SETTING_AHRS_USE_MAG_NO_GPS_DEFAULT
+    .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT
 );
 
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@@ -356,6 +356,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
     if (magBF || useCOG) {
         float wMag = 1.0f;
         float wCoG = 1.0f;
+        if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;}
+
         fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
         fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 
@@ -717,7 +719,7 @@ static void imuCalculateEstimatedAttitude(float dT)
         gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
     }
     // Use GPS (if available)
-    if (canUseCOG && (!(imuConfig()->use_mag_no_gps && useMag))) {
+    if (canUseCOG) {
         if (gpsHeadingInitialized) {
             courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
             useCOG = true;
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 24904b98ba6..8afcc50e579 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -54,7 +54,7 @@ typedef struct imuConfig_s {
     uint8_t acc_ignore_slope;
     uint8_t gps_yaw_windcomp;
     uint8_t inertia_comp_method;
-    uint8_t use_mag_no_gps;
+    uint16_t gps_yaw_weight;
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);

From 600d0add2f6cb66c9657a1790160a2c818e3b0de Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 03:59:52 +0900
Subject: [PATCH 13/22] update documents

---
 docs/Settings.md | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index e00265306b7..46309f86348 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -242,6 +242,16 @@ Inertial Measurement Unit KP Gain for compass measurements
 
 ---
 
+### ahrs_gps_yaw_weight
+
+Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 500 |
+
+---
+
 ### ahrs_gps_yaw_windcomp
 
 Wind compensation in heading estimation from gps groundcourse(fixed wing only)
@@ -1934,21 +1944,21 @@ _// TODO_
 
 ### inav_w_z_baro_p
 
-Weight of barometer measurements in estimated altitude and climb rate
+Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.35 | 0 | 10 |
+| 0.4 | 0 | 10 |
 
 ---
 
 ### inav_w_z_gps_p
 
-Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes
+Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.2 | 0 | 10 |
+| 0.4 | 0 | 10 |
 
 ---
 
@@ -1958,7 +1968,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.1 | 0 | 10 |
+| 0.6 | 0 | 10 |
 
 ---
 

From 195a062d27663ae286634cce81d7c3b1b22b983b Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 11:47:15 +0900
Subject: [PATCH 14/22] minor fix

---
 src/main/fc/settings.yaml                      | 2 +-
 src/main/navigation/navigation_pos_estimator.c | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 5d21793c0d3..63c0d15693e 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2336,7 +2336,7 @@ groups:
         field: w_z_gps_v
         min: 0
         max: 10
-        default_value: 0.6
+        default_value: 0.8
       - name: inav_w_xy_gps_p
         description: "Weight of GPS coordinates in estimated UAV position and speed."
         default_value: 1.0
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index f8473a354f9..e2e6b523045 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -572,7 +572,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
     bool correctOK = false;
     
-    const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro if difference is too big, baro is probably wrong
+    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; //ignore baro if difference is too big, baro is probably wrong
     //use both baro and gps
     if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) {
         timeUs_t currentTimeUs = micros();

From 4c4a74cdeffbe64f9ffbec32dd8f9b1cb48d1bb0 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 15:23:33 +0900
Subject: [PATCH 15/22] fix pos estimator acc weight

---
 .../navigation/navigation_pos_estimator.c     | 33 ++++++++++---------
 src/main/sensors/acceleration.h               |  2 +-
 2 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index e2e6b523045..2fccdfa980f 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -373,18 +373,19 @@ static bool gravityCalibrationComplete(void)
 
 static void updateIMUEstimationWeight(const float dt)
 {
-    bool isAccClipped = accIsClipped();
-
-    // If accelerometer measurement is clipped - drop the acc weight to zero
+    static float acc_clip_factor = 1.0f;
+    // If accelerometer measurement is clipped - drop the acc weight to 0.3
     // and gradually restore weight back to 1.0 over time
-    if (isAccClipped) {
-        posEstimator.imu.accWeightFactor = 0.0f;
+    if (accIsClipped()) {
+        acc_clip_factor = 0.5f;
     }
     else {
         const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
-        posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
+        acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
     }
-
+    // Update accelerometer weight based on vibration levels and clipping
+    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),1.0f,4.0f),1.0f,2.0f,1.0f,0.3f); // g/s
+    posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
     // DEBUG_VIBE[0-3] are used in IMU
     DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
 }
@@ -534,13 +535,12 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 
 static void estimationPredict(estimationContext_t * ctx)
 {
-    const float accWeight = navGetAccelerometerWeight();
 
     /* Prediction step: Z-axis */
     if ((ctx->newFlags & EST_Z_VALID)) {
         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
-        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
-        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
+        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
+        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
     }
 
     /* Prediction step: XY-axis */
@@ -551,10 +551,10 @@ static void estimationPredict(estimationContext_t * ctx)
 
         // If heading is valid, accelNEU is valid as well. Account for acceleration
         if (navIsHeadingUsable() && navIsAccelerationUsable()) {
-            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
-            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
-            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
-            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
+            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
+            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
+            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
+            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
         }
     }
 }
@@ -754,7 +754,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
         ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
     }
-
+    // Boost the corrections based on accWeight
+    const float accWeight = navGetAccelerometerWeight();
+    vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
+    vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
     // Apply corrections
     vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index d81a83a9084..ff643f4a153 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -27,7 +27,7 @@
 #define GRAVITY_CMSS    980.665f
 #define GRAVITY_MSS     9.80665f
 
-#define ACC_CLIPPING_THRESHOLD_G        7.9f
+#define ACC_CLIPPING_THRESHOLD_G        15.9f
 #define ACC_VIBE_FLOOR_FILT_HZ          5.0f
 #define ACC_VIBE_FILT_HZ                2.0f
 

From 3978947777666e9decbdf9ac36df916e578a746c Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 25 Oct 2023 15:25:00 +0900
Subject: [PATCH 16/22] update docs

---
 docs/Settings.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 46309f86348..4007ed1f654 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1968,7 +1968,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.6 | 0 | 10 |
+| 0.8 | 0 | 10 |
 
 ---
 

From 8e7cc6bb32c34950d8074c1b47805d986978cd65 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Thu, 26 Oct 2023 00:16:33 +0900
Subject: [PATCH 17/22] fixes display issue in configurator sensors tab

---
 src/main/fc/fc_msp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index cdaac609baa..c827e07890d 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -477,7 +477,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
             for (int i = 0; i < 3; i++) {
 #ifdef USE_MAG
-                sbufWriteU16(dst, mag.magADC[i]);
+                sbufWriteU16(dst, lrintf(mag.magADC[i]));
 #else
                 sbufWriteU16(dst, 0);
 #endif

From eb6634da67b40cd7cb1860e3fbf5944bc9f6ca7d Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Fri, 27 Oct 2023 00:56:26 +0900
Subject: [PATCH 18/22] minor fix on cog yaw estimation

add acc vibration to blackbox

adjustments in imu cog

minor fix on blackbox
---
 src/main/blackbox/blackbox.c    | 5 +++++
 src/main/flight/imu.c           | 7 ++++---
 src/main/sensors/acceleration.c | 3 ++-
 src/main/sensors/acceleration.h | 1 +
 4 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index cb12a7a8968..ae64a3e35a2 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -310,6 +310,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"accSmooth",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
+    {"accVib",     -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"attitude",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
     {"attitude",    1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
     {"attitude",    2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
@@ -487,6 +488,7 @@ typedef struct blackboxMainState_s {
     int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
 
     int16_t accADC[XYZ_AXIS_COUNT];
+    int16_t accVib;
     int16_t attitude[XYZ_AXIS_COUNT];
     int32_t debug[DEBUG32_VALUE_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
@@ -917,6 +919,7 @@ static void writeIntraframe(void)
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
         blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
+        blackboxWriteUnsignedVB(blackboxCurrent->accVib);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@@ -1182,6 +1185,7 @@ static void writeInterframe(void)
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
         blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
+        blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@@ -1601,6 +1605,7 @@ static void loadMainState(timeUs_t currentTimeUs)
             blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
         }
     }
+    blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
 
     if (STATE(FIXED_WING_LEGACY)) {
 
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index a5eba967501..afdc86e6bf9 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -49,6 +49,7 @@
 
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/mixer_profile.h"
 #include "flight/pid.h"
 #if defined(USE_WIND_ESTIMATOR)
 #include "flight/wind_estimator.h"
@@ -78,7 +79,7 @@
 #define SPIN_RATE_LIMIT             20
 #define MAX_ACC_NEARNESS            0.2    // 20% or G error soft-accepted (0.8-1.2G)
 #define MAX_MAG_NEARNESS            0.25    // 25% or magnetic field error soft-accepted (0.75-1.25)
-#define COS5DEG 0.996f
+#define COS10DEG 0.985f
 #define COS20DEG 0.940f
 #define IMU_ROTATION_LPF         3       // Hz
 FASTRAM fpVector3_t imuMeasuredAccelBF;
@@ -397,7 +398,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         if (useCOG) {
             fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
             //vForward as trust vector
-            if (STATE(MULTIROTOR)){
+            if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
                 vForward.z = 1.0f;
             }else{
                 vForward.x = 1.0f;
@@ -434,7 +435,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
                 wCoG *= imuCalculateMcCogWeight();
                 //scale accroading to multirotor`s tilt angle
-                wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS5DEG), COS20DEG, COS5DEG, 1.0f, 0.0f);
+                wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f);
                 //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed
             }
             vHeadingEF.z = 0.0f;
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index f897d7e580a..695f49b00b2 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -576,6 +576,7 @@ void accUpdate(void)
         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
         const float accDiff = acc.accADCf[axis] - accFloorFilt;
         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
+        acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
     }
 
     // Filter acceleration
@@ -612,7 +613,7 @@ void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 
 float accGetVibrationLevel(void)
 {
-    return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
+    return acc.accVibe;
 }
 
 uint32_t accGetClipCount(void)
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index ff643f4a153..f3385edc6d5 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -58,6 +58,7 @@ typedef struct acc_s {
     uint32_t accTargetLooptime;
     float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
     float accVibeSq[XYZ_AXIS_COUNT];
+    float accVibe;
     uint32_t accClipCount;
     bool isClipped;
     acc_extremes_t extremes[XYZ_AXIS_COUNT];

From b911185a881c03e53c0a30d4b2131444fa7405a0 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Fri, 27 Oct 2023 12:03:25 +0900
Subject: [PATCH 19/22] adjustments

---
 docs/Settings.md                               | 10 ----------
 src/main/fc/settings.yaml                      |  5 -----
 src/main/navigation/navigation.h               |  1 -
 src/main/navigation/navigation_pos_estimator.c |  9 ++++-----
 4 files changed, 4 insertions(+), 21 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index ca98e074c29..c1a109f3593 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1922,16 +1922,6 @@ Decay coefficient for estimated velocity when GPS reference for position is lost
 
 ---
 
-### inav_w_xyz_acc_p
-
-_// TODO_
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 1.0 | 0 | 1 |
-
----
-
 ### inav_w_z_baro_p
 
 Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 91292198daf..90e7cff250b 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2366,11 +2366,6 @@ groups:
         field: w_xy_res_v
         min: 0
         max: 10
-      - name: inav_w_xyz_acc_p
-        field: w_xyz_acc_p
-        min: 0
-        max: 1
-        default_value: 1.0
       - name: inav_w_acc_bias
         description: "Weight for accelerometer drift estimation"
         default_value: 0.01
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index ecc487cc9e7..57f8a192b41 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -203,7 +203,6 @@ typedef struct positionEstimationConfig_s {
     float w_xy_res_v;
 
     float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
-    float w_xyz_acc_p;
 
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     float baro_epv;     // Baro position error
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index a18b6abec15..1b561e98810 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
-        .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
-
         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
 
         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
@@ -389,7 +387,8 @@ static bool gravityCalibrationComplete(void)
 
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
-
+#define ACC_VIB_FACTOR_S 1.0f
+#define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
 {
     static float acc_clip_factor = 1.0f;
@@ -403,7 +402,7 @@ static void updateIMUEstimationWeight(const float dt)
         acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
     }
     // Update accelerometer weight based on vibration levels and clipping
-    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),1.0f,4.0f),1.0f,2.0f,1.0f,0.3f); // g/s
+    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s
     posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
     // DEBUG_VIBE[0-3] are used in IMU
     DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
@@ -411,7 +410,7 @@ static void updateIMUEstimationWeight(const float dt)
 
 float navGetAccelerometerWeight(void)
 {
-    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    const float accWeightScaled = posEstimator.imu.accWeightFactor;
     DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
 
     return accWeightScaled;

From 3a9624ba492c84afb7ff103466aefa017849fc53 Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Fri, 3 Nov 2023 21:30:41 +0900
Subject: [PATCH 20/22] sync updateActualHeading with other topic

---
 .../navigation/navigation_pos_estimator.c     | 22 ++++++++-----------
 .../navigation_pos_estimator_private.h        |  1 -
 2 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 1b561e98810..8157fa730cd 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -707,15 +707,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
 
 static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
     if (STATE(GPS_FIX) && navIsHeadingUsable()) {
-        static timeUs_t lastUpdateTimeUs = 0;
-
-        if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) {   // limit update rate
-            const float dt = US2S(currentTimeUs - lastUpdateTimeUs);
-            uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt)));
-            posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
-            lastUpdateTimeUs = currentTimeUs;
-        }
+        uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x)));
+        posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
     }
 }
 
@@ -813,13 +808,14 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 {
     static navigationTimer_t posPublishTimer;
 
-    /* IMU operates in decidegrees while INAV operates in deg*100
-     * Use course over ground when GPS heading valid */
-    int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
-    updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
-
     /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
     if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
+        /* Publish heading update */
+        /* IMU operates in decidegrees while INAV operates in deg*100
+        * Use course over ground when GPS heading valid */
+        int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
+        updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
+
         /* Publish position update */
         if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
             // FIXME!!!!!
diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h
index 40129f844b0..0b19dbb0023 100644
--- a/src/main/navigation/navigation_pos_estimator_private.h
+++ b/src/main/navigation/navigation_pos_estimator_private.h
@@ -39,7 +39,6 @@
 
 #define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
 #define INAV_PITOT_UPDATE_RATE              10
-#define INAV_COG_UPDATE_RATE_HZ             20      // ground course update rate
 
 #define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
 #define INAV_BARO_TIMEOUT_MS                200     // Baro timeout

From 6f1ad0a9f3302e01046f8c93eddf3227f5fff8ed Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Sat, 4 Nov 2023 02:01:29 +0900
Subject: [PATCH 21/22] add more gps dynamic model

---
 docs/Settings.md          |  4 ++--
 src/main/fc/settings.yaml |  6 +++---
 src/main/io/gps.h         |  2 ++
 src/main/io/gps_ublox.c   | 10 ++++++++--
 src/main/io/gps_ublox.h   |  2 ++
 5 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index cb760e312ab..1f0fe29c419 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1484,11 +1484,11 @@ Enable automatic configuration of UBlox GPS receivers.
 
 ### gps_dyn_model
 
-GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying.
+GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| AIR_1G |  |  |
+| AIR_2G |  |  |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 9fb1c051a94..eed0f699744 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -50,7 +50,7 @@ tables:
     values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]
     enum: sbasMode_e
   - name: gps_dyn_model
-    values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"]
+    values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"]
     enum: gpsDynModel_e
   - name: reset_type
     values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
@@ -1614,8 +1614,8 @@ groups:
         table: gps_sbas_mode
         type: uint8_t
       - name: gps_dyn_model
-        description: "GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying."
-        default_value: "AIR_1G"
+        description: "GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying."
+        default_value: "AIR_2G"
         field: dynModel
         table: gps_dyn_model
         type: uint8_t
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 199652a3a24..3e9d073c54c 100755
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -76,7 +76,9 @@ typedef enum {
 
 typedef enum {
     GPS_DYNMODEL_PEDESTRIAN = 0,
+    GPS_DYNMODEL_AUTOMOTIVE,
     GPS_DYNMODEL_AIR_1G,
+    GPS_DYNMODEL_AIR_2G,
     GPS_DYNMODEL_AIR_4G,
 } gpsDynModel_e;
 
diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index d03c3390776..a9455e003ba 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -800,10 +800,16 @@ STATIC_PROTOTHREAD(gpsConfigure)
         case GPS_DYNMODEL_PEDESTRIAN:
             configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
             break;
-        case GPS_DYNMODEL_AIR_1G:   // Default to this
-        default:
+        case GPS_DYNMODEL_AUTOMOTIVE:
+            configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO);
+            break;
+        case GPS_DYNMODEL_AIR_1G:
             configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
             break;
+        case GPS_DYNMODEL_AIR_2G:   // Default to this
+        default:
+            configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO);
+            break;
         case GPS_DYNMODEL_AIR_4G:
             configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
             break;
diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h
index 00b42eeb2b7..8ec9be16d09 100644
--- a/src/main/io/gps_ublox.h
+++ b/src/main/io/gps_ublox.h
@@ -34,7 +34,9 @@ extern "C" {
 #define GPS_CAPA_INTERVAL                   5000
 
 #define UBX_DYNMODEL_PEDESTRIAN 3
+#define UBX_DYNMODEL_AUTOMOVITE 4
 #define UBX_DYNMODEL_AIR_1G     6
+#define UBX_DYNMODEL_AIR_2G     7
 #define UBX_DYNMODEL_AIR_4G     8
 
 #define UBX_FIXMODE_2D_ONLY 1

From 745017d058f6929e52fa97750ae0179d920095bb Mon Sep 17 00:00:00 2001
From: shota <miyafly7@gmail.com>
Date: Wed, 8 Nov 2023 19:03:41 +0900
Subject: [PATCH 22/22] Add fade out effect to baro altitude correction

---
 src/main/navigation/navigation_pos_estimator.c | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 8157fa730cd..f66c641928b 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -590,9 +590,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
     bool correctOK = false;
     
-    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; //ignore baro if difference is too big, baro is probably wrong
+    //ignore baro if difference is too big, baro is probably wrong
+    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+    //fade out the baro to prevent sudden jump
+    const float start_epv = positionEstimationConfig()->max_eph_epv;
+    const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+    const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
     //use both baro and gps
-    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) {
+    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -616,14 +621,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
         // Altitude
         const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
-        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
+        ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
 
         ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 
         // Accelerometer bias
         if (!isAirCushionEffectDetected) {
-            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+            ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
         correctOK = true;