Surface estimation tweaks

src/main/common/maths.c

@@ -187,7 +187,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
     return ((a / b) - (destMax - destMin)) + destMax;
 }
 
-int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
+float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
     float a = (destMax - destMin) * (x - srcMin);
     float b = srcMax - srcMin;
     return ((a / b) - (destMax - destMin)) + destMax;

src/main/common/maths.h

@@ -144,7 +144,7 @@ float devStandardDeviation(stdev_t *dev);
 float degreesToRadians(int16_t degrees);
 
 int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
-int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
+float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
 
 void normalizeV(struct fp_vector *src, struct fp_vector *dest);
 

src/main/fc/cli.c

@@ -783,6 +783,10 @@ static const clivalue_t valueTable[] = {
     { "inav_gps_delay",             VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0,  500 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, gps_delay_ms) },
     { "inav_reset_altitude",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_NAV_RESET_ALTITUDE }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, use_gps_velned) },
 
+    { "inav_max_sonar_altitude",    VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0,  1000 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, max_sonar_altitude) },
+
+    { "inav_w_z_sonar_p",           VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0,  10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_sonar_p) },
+    { "inav_w_z_sonar_v",           VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0,  10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_sonar_v) },
     { "inav_w_z_baro_p",            VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0,  10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_baro_p) },
     { "inav_w_z_gps_p",             VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0,  10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_p) },
     { "inav_w_z_gps_v",             VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0,  10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_v) },

src/main/navigation/navigation.c

@@ -2728,7 +2728,7 @@ void navigationUsePIDs(void)
 
     // Initialize surface tracking PID
     navPidInit(&posControl.pids.surface, 2.0f,
-                                         1.0f,
+                                         0.0f,
                                          0.0f);
 
     /** Airplane PIDs */

src/main/navigation/navigation.h

@@ -72,6 +72,8 @@ typedef struct positionEstimationConfig_s {
     uint8_t use_gps_velned;
     uint16_t gps_delay_ms;
 
+    uint16_t max_sonar_altitude;
+
     float w_z_baro_p;   // Weight (cutoff frequency) for barometer altitude measurements
 
     float w_z_sonar_p;  // Weight (cutoff frequency) for sonar altitude measurements

src/main/navigation/navigation_multicopter.c

@@ -65,13 +65,12 @@ static void updateSurfaceTrackingAltitudeSetpoint(timeDelta_t deltaMicros)
     if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) {
         if (posControl.actualState.surface >= 0 && posControl.flags.hasValidSurfaceSensor) {
             // We better overshoot a little bit than undershoot
-            const float targetAltitudeError = navPidApply2(&posControl.pids.surface, posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), -5.0f, +35.0f, 0);
+            const float targetAltitudeError = navPidApply2(&posControl.pids.surface, posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), -35.0f, +35.0f, 0);
             posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + targetAltitudeError;
         }
         else {
             // TODO: We are possible above valid range, we now descend down to attempt to get back within range
-            //updateAltitudeTargetFromClimbRate(-0.10f * navConfig()->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET);
+            updateAltitudeTargetFromClimbRate(-50.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
-            updateAltitudeTargetFromClimbRate(-20.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
         }
     }
 

src/main/navigation/navigation_pos_estimator.c

@@ -80,9 +80,6 @@
 #define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
 #define INAV_SONAR_TIMEOUT_MS               300     // Sonar timeout    (missed 3 readings in a row)
 
-#define INAV_SONAR_W1                       3.000f
-#define INAV_SONAR_W2                       4.700f
-
 #define INAV_HISTORY_BUF_SIZE               (INAV_POSITION_PUBLISH_RATE_HZ / 2)     // Enough to hold 0.5 sec historical data
 
 typedef struct {
@@ -168,7 +165,7 @@ typedef struct {
 
 static navigationPosEstimator_s posEstimator;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 1);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
@@ -178,8 +175,13 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .accz_unarmed_cal = 1,
         .use_gps_velned = 1,         // "Disabled" is mandatory with gps_dyn_model = Pedestrian
 
+        .max_sonar_altitude = 200,
+
         .w_z_baro_p = 0.35f,
 
+        .w_z_sonar_p = 3.000f,
+        .w_z_sonar_v = 4.700f,
+
         .w_z_gps_p = 0.2f,
         .w_z_gps_v = 0.5f,
 
@@ -479,7 +481,7 @@ void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs)
     newSonarAlt = rangefinderCalculateAltitude(newSonarAlt, calculateCosTiltAngle());
 
     /* Apply predictive filter to sonar readings */
-    if (newSonarAlt > 0 && newSonarAlt <= INAV_SONAR_MAX_DISTANCE) {
+    if (newSonarAlt > 0 && newSonarAlt <= positionEstimationConfig()->max_sonar_altitude) {
         float sonarDt = (currentTimeUs - posEstimator.sonar.lastUpdateTime) * 1e-6;
         posEstimator.sonar.lastUpdateTime = currentTimeUs;
 
@@ -492,14 +494,12 @@ void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs)
             posEstimator.sonar.alt = posEstimator.sonar.alt + posEstimator.sonar.vel * sonarDt;
 
             const float sonarResidual = newSonarAlt - posEstimator.sonar.alt;
+            const float bellCurveScaler = scaleRangef(bellCurve(sonarResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
 
-            posEstimator.sonar.alt += sonarResidual * INAV_SONAR_W1 * sonarDt;
+            posEstimator.sonar.alt += sonarResidual * positionEstimationConfig()->w_z_sonar_p * bellCurveScaler * sonarDt;
-            posEstimator.sonar.vel += sonarResidual * INAV_SONAR_W2 * sonarDt;
+            posEstimator.sonar.vel += sonarResidual * positionEstimationConfig()->w_z_sonar_v * sq(bellCurveScaler) * sonarDt;
         }
     }
-
-    debug[0] = posEstimator.sonar.alt;
-    debug[1] = posEstimator.sonar.vel;
 }
 #endif