[navigation for multirotor] Sqrt Controller for Alt-Hold and improvements

docs/development/IDE - Visual Studio Code with Windows 10.md

@@ -40,7 +40,6 @@ Edit file `./.vscode/c_cpp_properties.json` to setup enabled `defines`
             "cStandard": "c11",
             "cppStandard": "c++17",
             "defines": [
-                "USE_NAV",
                 "NAV_FIXED_WING_LANDING",
                 "USE_OSD",
                 "USE_GYRO_NOTCH_1",

src/main/CMakeLists.txt

@@ -552,6 +552,8 @@ main_sources(COMMON_SRC
     navigation/navigation_pos_estimator_flow.c
     navigation/navigation_private.h
     navigation/navigation_rover_boat.c
+    navigation/sqrt_controller.c
+    navigation/sqrt_controller.h
 
     sensors/barometer.c
     sensors/barometer.h

src/main/common/fp_pid.c

@@ -47,7 +47,13 @@ float navPidApply3(
     const float dTermScaler
 ) {
     float newProportional, newDerivative, newFeedForward;
-    float error = setpoint - measurement;
+    float error = 0.0f;
+
+    if (pid->errorLpfHz > 0.0f) {
+        error = pt1FilterApply4(&pid->error_filter_state, setpoint - measurement, pid->errorLpfHz, dt);
+    } else {
+        error = setpoint - measurement;
+    }
 
     /* P-term */
     newProportional = error * pid->param.kP * gainScaler;
@@ -68,7 +74,7 @@ float navPidApply3(
         pid->last_input = measurement;
     }
 
-    if (pid->dTermLpfHz > 0) {
+    if (pid->dTermLpfHz > 0.0f) {
         newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
     } else {
         newDerivative = pid->param.kD * newDerivative;
@@ -138,7 +144,7 @@ void navPidReset(pidController_t *pid)
     pid->output_constrained = 0.0f;
 }
 
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz)
 {
     pid->param.kP = _kP;
     pid->param.kI = _kI;
@@ -159,5 +165,6 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
         pid->param.kT = 0.0;
     }
     pid->dTermLpfHz = _dTermLpfHz;
+    pid->errorLpfHz = _errorLpfHz;
     navPidReset(pid);
 }

src/main/common/fp_pid.h

@@ -48,7 +48,9 @@ typedef enum {
 typedef struct {
     bool reset;
     pidControllerParam_t param;
+    pt1Filter_t error_filter_state;
     pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
+    float errorLpfHz;
     float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
     float integrator;                   // integrator value
     float last_input;                   // last input for derivative
@@ -73,4 +75,4 @@ float navPidApply3(
     const float dTermScaler
 );
 void navPidReset(pidController_t *pid);
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz);

src/main/fc/settings.yaml

@@ -1978,7 +1978,6 @@ groups:
         min: HEADING_HOLD_RATE_LIMIT_MIN
         max: HEADING_HOLD_RATE_LIMIT_MAX
         default_value: 90
-
       - name: nav_mc_pos_z_p
         description: "P gain of altitude PID controller (Multirotor)"
         field: bank_mc.pid[PID_POS_Z].P
@@ -1999,7 +1998,6 @@ groups:
         default_value: 50
       - name: nav_mc_vel_z_d
         description: "D gain of velocity PID controller"
-        default_value: 10
         field: bank_mc.pid[PID_VEL_Z].D
         min: 0
         max: 255

src/main/flight/pid.c

@@ -1260,7 +1260,8 @@ void pidInit(void)
         (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
         0.0f,
         0.0f,
-        2.0f
+        2.0f,
+        0.0f
     );
 
 }

src/main/navigation/navigation.c

@@ -3727,14 +3727,16 @@ void navigationUsePIDs(void)
             0.0f,
             0.0f,
             0.0f,
-            NAV_DTERM_CUT_HZ
+            NAV_DTERM_CUT_HZ,
+            0.0f
         );
 
         navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
-                                               pidProfile()->navVelXyDTermLpfHz
+                                               pidProfile()->navVelXyDTermLpfHz,
+                                               0.0f
         );
     }
 
@@ -3756,22 +3758,25 @@ void navigationUsePIDs(void)
         0.0f,
         0.0f,
         0.0f,
-        NAV_DTERM_CUT_HZ
+        NAV_DTERM_CUT_HZ,
+        0.0f
     );
 
     navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
                                         0.0f,
-                                        NAV_DTERM_CUT_HZ
+                                        NAV_VEL_Z_DERIVATIVE_CUT_HZ,
+                                        NAV_VEL_Z_ERROR_CUT_HZ
     );
 
     // Initialize surface tracking PID
     navPidInit(&posControl.pids.surface, 2.0f,
                                          0.0f,
                                          0.0f,
                                          0.0f,
-                                         NAV_DTERM_CUT_HZ
+                                         NAV_DTERM_CUT_HZ,
+                                         0.0f
     );
 
     /** Airplane PIDs */
@@ -3780,21 +3785,24 @@ void navigationUsePIDs(void)
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
                                         0.0f,
-                                        NAV_DTERM_CUT_HZ
+                                        NAV_DTERM_CUT_HZ,
+                                        0.0f
     );
 
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
                                         0.0f,
-                                        NAV_DTERM_CUT_HZ
+                                        NAV_DTERM_CUT_HZ,
+                                        0.0f
     );
 
     navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
                                         0.0f,
-                                        2.0f
+                                        2.0f,
+                                        0.0f
     );
 }
 

src/main/navigation/navigation.h

@@ -375,7 +375,7 @@ typedef struct navDestinationPath_s {
 
 typedef struct navigationPIDControllers_s {
     /* Multicopter PIDs */
-    pidController_t   pos[XYZ_AXIS_COUNT];
+    pidController_t pos[XYZ_AXIS_COUNT];
     pidController_t vel[XYZ_AXIS_COUNT];
     pidController_t surface;
 

src/main/navigation/navigation_multicopter.c

@@ -48,31 +48,45 @@
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
+#include "navigation/sqrt_controller.h"
 
 #include "sensors/battery.h"
 
 /*-----------------------------------------------------------
  * Altitude controller for multicopter aircraft
  *-----------------------------------------------------------*/
+
 static int16_t rcCommandAdjustedThrottle;
 static int16_t altHoldThrottleRCZero = 1500;
 static pt1Filter_t altholdThrottleFilterState;
 static bool prepareForTakeoffOnReset = false;
+static sqrt_controller_t alt_hold_sqrt_controller;
 
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 {
-    const float altitudeError = posControl.desiredState.pos.z - navGetCurrentActualPositionAndVelocity()->pos.z;
-    float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
+    float pos_desired_z = posControl.desiredState.pos.z;
+
+    float targetVel = sqrtControllerApply(
+        &alt_hold_sqrt_controller,
+        &pos_desired_z,
+        navGetCurrentActualPositionAndVelocity()->pos.z,
+        US2S(deltaMicros)
+    );
 
+    posControl.desiredState.pos.z = pos_desired_z;
+
     // hard limit desired target velocity to max_climb_rate
+    float vel_max_z = 0.0f;
+
     if (posControl.flags.isAdjustingAltitude) {
-        targetVel = constrainf(targetVel, -navConfig()->general.max_manual_climb_rate, navConfig()->general.max_manual_climb_rate);
-    }
-    else {
-        targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate);
+        vel_max_z = navConfig()->general.max_manual_climb_rate;
+    } else {
+        vel_max_z = navConfig()->general.max_auto_climb_rate;
     }
 
+    targetVel = constrainf(targetVel, -vel_max_z, vel_max_z);
+
     posControl.pids.pos[Z].output_constrained = targetVel;
 
     // Limit max up/down acceleration target
@@ -104,11 +118,14 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
     // Calculate min and max throttle boundaries (to compensate for integral windup)
     const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
     const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
-
-    posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
-
-    posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+
+    float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
+
+    posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+
     posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
+
+    posControl.rcAdjustment[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
 }
 
 bool adjustMulticopterAltitudeFromRCInput(void)
@@ -191,14 +208,40 @@ void setupMulticopterAltitudeController(void)
 
 void resetMulticopterAltitudeController(void)
 {
-    const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity();
+    const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
+    float nav_speed_up = 0.0f;
+    float nav_speed_down = 0.0f;
+    float nav_accel_z = 0.0f;
 
     navPidReset(&posControl.pids.vel[Z]);
     navPidReset(&posControl.pids.surface);
+
     posControl.rcAdjustment[THROTTLE] = 0;
 
     posControl.desiredState.vel.z = posToUse->vel.z;   // Gradually transition from current climb
+
     pt1FilterReset(&altholdThrottleFilterState, 0.0f);
+    pt1FilterReset(&posControl.pids.vel[Z].error_filter_state, 0.0f);
+    pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f);
+
+    if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
+        const float maxSpeed = getActiveWaypointSpeed();
+        nav_speed_up = maxSpeed;
+        nav_accel_z = maxSpeed;
+        nav_speed_down = navConfig()->general.max_auto_climb_rate;
+    } else {
+        nav_speed_up = navConfig()->general.max_manual_speed;
+        nav_accel_z = navConfig()->general.max_manual_speed;
+        nav_speed_down = navConfig()->general.max_manual_climb_rate;
+    }
+
+    sqrtControllerInit(
+        &alt_hold_sqrt_controller,
+        posControl.pids.pos[Z].param.kP,
+        -fabsf(nav_speed_down), 
+        nav_speed_up, 
+        nav_accel_z
+    );
 }
 
 static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
@@ -214,7 +257,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
         if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
             // If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump
             if (prepareForTakeoffOnReset) {
-                const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity();
+                const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
 
                 posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
                 posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
@@ -237,7 +280,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
     }
 
     // Update throttle controller
-    rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
 
     // Save processed throttle for future use
     rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@@ -777,7 +820,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     }
 
     // Update throttle controller
-    rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_private.h

@@ -31,6 +31,8 @@
 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
 #define NAV_FW_CONTROL_MONITORING_RATE      2
 #define NAV_DTERM_CUT_HZ                    10.0f
+#define NAV_VEL_Z_DERIVATIVE_CUT_HZ 5.0f
+#define NAV_VEL_Z_ERROR_CUT_HZ 5.0f
 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
 
 #define INAV_SURFACE_MAX_DISTANCE           40