SURFACE: Improve surface tracking

src/main/blackbox/blackbox.c

@@ -238,6 +238,8 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"navTgtPos",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"navTgtPos",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"navTgtPos",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+    {"navSurf",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+    {"navTgtSurf", 0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"navDebug",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"navDebug",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"navDebug",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@@ -329,6 +331,8 @@ typedef struct blackboxMainState_s {
     int16_t navTargetPos[XYZ_AXIS_COUNT];
     int16_t navHeading;
     int16_t navTargetHeading;
+    int16_t navSurface;
+    int16_t navTargetSurface;
     int16_t navDebug[4];
 #endif
 } blackboxMainState_t;
@@ -625,6 +629,9 @@ static void writeIntraframe(void)
         blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
     }
 
+    blackboxWriteSignedVB(blackboxCurrent->navSurface);
+    blackboxWriteSignedVB(blackboxCurrent->navTargetSurface);
+
     for (x = 0; x < 4; x++) {
         blackboxWriteSignedVB(blackboxCurrent->navDebug[x]);
     }
@@ -770,6 +777,9 @@ static void writeInterframe(void)
         blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - (blackboxHistory[1]->navTargetPos[x] + blackboxHistory[2]->navTargetPos[x]) / 2);
     }
 
+    blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
+    blackboxWriteSignedVB(blackboxCurrent->navTargetSurface - blackboxLast->navTargetSurface);
+
     for (x = 0; x < 4; x++) {
         blackboxWriteSignedVB(blackboxCurrent->navDebug[x] - blackboxLast->navDebug[x]);
     }
@@ -1063,6 +1073,8 @@ static void loadMainState(void)
         blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
         blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
     }
+    blackboxCurrent->navSurface = navActualSurface;
+    blackboxCurrent->navTargetSurface = navTargetSurface;
     for (i = 0; i < 4; i++) {
         blackboxCurrent->navDebug[i] = navDebug[i];
     }

src/main/flight/navigation_rewrite.c

@@ -66,6 +66,8 @@ int16_t navDesiredHeading;
 int16_t navTargetPosition[3];
 int32_t navLatestActualPosition[3];
 int16_t navDebug[4];
+int16_t navTargetSurface;
+int16_t navActualSurface;
 uint16_t navFlags;
 #endif
 
@@ -1359,6 +1361,10 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo
     else {
         posControl.flags.surfaceDistanceNewData = 0;
     }
+
+#if defined(NAV_BLACKBOX)
+    navActualSurface = surfaceDistance;
+#endif
 }
 
 /*-----------------------------------------------------------
@@ -1503,11 +1509,15 @@ void updateHomePosition(void)
 void setDesiredSurfaceOffset(float surfaceOffset)
 {
     if (surfaceOffset > 0) {
-        posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SONAR_MAX_DISTANCE);
+        posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SURFACE_MAX_DISTANCE);
     }
     else {
         posControl.desiredState.surface = -1;
     }
+
+#if defined(NAV_BLACKBOX)
+    navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
+#endif
 }
 
 /*-----------------------------------------------------------
@@ -1556,6 +1566,7 @@ void setDesiredPosition(t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t us
     navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
     navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
     navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
+    navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
 #endif
 }
 
@@ -1605,7 +1616,7 @@ void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRat
     else {
         if (posControl.flags.isTerrainFollowEnabled) {
             if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) {
-                posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SONAR_MAX_DISTANCE);
+                posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SURFACE_MAX_DISTANCE);
             }
         }
         else {
@@ -1617,6 +1628,7 @@ void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRat
 
 #if defined(NAV_BLACKBOX)
     navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
+    navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
 #endif
 }
 
@@ -2167,6 +2179,11 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile)
                                         (float)posControl.pidProfile->I8[PIDVEL] / 100.0f,
                                         (float)posControl.pidProfile->D8[PIDVEL] / 100.0f);
 
+    // Initialize surface tracking PID
+    navPidInit(&posControl.pids.surface, 1.0f,
+                                         0.25f,
+                                         0.0f);
+
     // Initialize fixed wing PID controllers
     navPidInit(&posControl.pids.fw_nav, (float)posControl.pidProfile->P8[PIDNAVR] / 100.0f,
                                         (float)posControl.pidProfile->I8[PIDNAVR] / 100.0f,

src/main/flight/navigation_rewrite.h

@@ -273,6 +273,8 @@ extern int16_t navActualVelocity[3];
 extern int16_t navDesiredVelocity[3];
 extern int16_t navTargetPosition[3];
 extern int32_t navLatestActualPosition[3];
+extern int16_t navTargetSurface;
+extern int16_t navActualSurface;
 extern int16_t navDebug[4];
 extern uint16_t navFlags;
 #define NAV_BLACKBOX_DEBUG(x,y) navDebug[x] = constrain((y), -32678, 32767)

src/main/flight/navigation_rewrite_multicopter.c

@@ -62,24 +62,25 @@ static int16_t rcCommandAdjustedThrottle;
 static int16_t altHoldThrottleRCZero = 1500;
 static filterStatePt1_t altholdThrottleFilterState;
 
-#define SURFACE_TRACKING_GAIN       40.0f
-
 /* Calculate global altitude setpoint based on surface setpoint */
 static void updateSurfaceTrackingAltitudeSetpoint(uint32_t deltaMicros)
 {
     /* If we have a surface offset target and a valid surface offset reading - recalculate altitude target */
     if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) {
         if (posControl.actualState.surface >= 0 && posControl.flags.hasValidSurfaceSensor) {
-            float surfaceOffsetError = posControl.desiredState.surface - posControl.actualState.surface;
+            float targetAltitudeError = navPidApply2(posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), &posControl.pids.surface, -25.0f, +25.0f, false);
-            float trackingWeight = SURFACE_TRACKING_GAIN * US2S(deltaMicros);
+            posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + targetAltitudeError;
-
-            posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + surfaceOffsetError * trackingWeight;
         }
         else {
             // TODO: We are possible above valid range, we now descend down to attempt to get back within range
-            updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET);
+            //updateAltitudeTargetFromClimbRate(-0.10f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET);
+            updateAltitudeTargetFromClimbRate(-20.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
         }
     }
+
+#if defined(NAV_BLACKBOX)
+    navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
+#endif
 }
 
 // Position to velocity controller for Z axis
@@ -169,6 +170,7 @@ void setupMulticopterAltitudeController(void)
 void resetMulticopterAltitudeController()
 {
     navPidReset(&posControl.pids.vel[Z]);
+    navPidReset(&posControl.pids.surface);
     filterResetPt1(&altholdThrottleFilterState, 0.0f);
     posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z;   // Gradually transition from current climb
     posControl.rcAdjustment[THROTTLE] = 0;

src/main/flight/navigation_rewrite_private.h

@@ -31,7 +31,8 @@
 #define NAV_DTERM_CUT_HZ                    10
 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
 
-#define INAV_SONAR_MAX_DISTANCE             50      // Sonar is unreliable above 50cm due to noise from propellers
+#define INAV_SONAR_MAX_DISTANCE             55      // Sonar is unreliable above 50cm due to noise from propellers
+#define INAV_SURFACE_MAX_DISTANCE           40
 
 #define HZ2US(hz)   (1000000 / (hz))
 #define US2S(us)    ((us) * 1e-6f)
@@ -101,6 +102,7 @@ typedef struct navigationPIDControllers_s {
     /* Multicopter PIDs */
     pController_t   pos[XYZ_AXIS_COUNT];
     pidController_t vel[XYZ_AXIS_COUNT];
+    pidController_t surface;
 
     /* Fixed-wing PIDs */
     pidController_t fw_alt;