Merge pull request #4603 from iNavFlight/de_yg_surface_mode_experimental

Add accelerometer weight to position estimation

src/main/fc/settings.yaml

@@ -1189,19 +1189,19 @@ groups:
       - name: inav_w_z_surface_p
         field: w_z_surface_p
         min: 0
-        max: 10
+        max: 100
       - name: inav_w_z_surface_v
         field: w_z_surface_v
         min: 0
-        max: 10
+        max: 100
       - name: inav_w_xy_flow_p
         field: w_xy_flow_p
         min: 0
-        max: 10
+        max: 100
       - name: inav_w_xy_flow_v
         field: w_xy_flow_v
         min: 0
-        max: 10
+        max: 100
       - name: inav_w_z_baro_p
         field: w_z_baro_p
         min: 0
@@ -1230,6 +1230,10 @@ 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
       - name: inav_w_acc_bias
         field: w_acc_bias
         min: 0

src/main/io/rangefinder_benewake.c

@@ -123,13 +123,6 @@ static void benewakeRangefinderUpdate(void)
                 if (sensorData == 0 || qual <= BENEWAKE_MIN_QUALITY) {
                     sensorData = -1;
                 }
-
-                /*
-                debug[0] = ((tfminiPacket_t *) &buffer[0])->distL;
-                debug[1] = ((tfminiPacket_t *) &buffer[0])->distH;
-                debug[2] = ((tfminiPacket_t *) &buffer[0])->strengthH;
-                debug[3] = ((tfminiPacket_t *) &buffer[0])->strengthL;
-                */
             }
 
             // Prepare for new packet

src/main/navigation/navigation.c

@@ -2687,6 +2687,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
     if (posControl.flags.estAltStatus == EST_TRUSTED)       navFlags |= (1 << 0);
     if (posControl.flags.estAglStatus == EST_TRUSTED)       navFlags |= (1 << 1);
     if (posControl.flags.estPosStatus == EST_TRUSTED)       navFlags |= (1 << 2);
+    if (posControl.flags.isTerrainFollowEnabled)            navFlags |= (1 << 3);
 #if defined(NAV_GPS_GLITCH_DETECTION)
     if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
 #endif

src/main/navigation/navigation.h

@@ -129,6 +129,7 @@ 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

src/main/navigation/navigation_pos_estimator.c

@@ -53,7 +53,7 @@
 
 navigationPosEstimator_t posEstimator;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 4);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
@@ -66,6 +66,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_surface_altitude = 200,
 
+        .w_xyz_acc_p = 1.0f,
+
         .w_z_baro_p = 0.35f,
 
         .w_z_surface_p = 3.500f,
@@ -433,6 +435,12 @@ static bool navIsHeadingUsable(void)
     }
 }
 
+float navGetAccelerometerWeight(void)
+{
+    // TODO(digitalentity): consider accelerometer health in weight calculation
+    return positionEstimationConfig()->w_xyz_acc_p;
+}
+
 static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 {
     /* Figure out if we have valid position data from our data sources */
@@ -474,11 +482,13 @@ 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;
+        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
-        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
     }
 
     /* Prediction step: XY-axis */
@@ -489,10 +499,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;
+            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;
+            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
-            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
+            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
-            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
+            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
         }
     }
 }

src/main/navigation/navigation_pos_estimator_agl.c

@@ -149,9 +149,10 @@ void estimationCalculateAGL(estimationContext_t * ctx)
         }
 
         // Update estimate
+        const float accWeight = navGetAccelerometerWeight();
         posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
-        posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
+        posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
-        posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt;
+        posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
 
         // Apply correction
         if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {

src/main/navigation/navigation_pos_estimator_private.h

@@ -182,5 +182,5 @@ typedef struct {
 extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
 extern void estimationCalculateAGL(estimationContext_t * ctx);
 extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
-
+extern float navGetAccelerometerWeight(void);