CR97 + CR47

src/main/blackbox/blackbox.c

@@ -472,7 +472,7 @@ typedef struct blackboxMainState_s {
     int32_t mcSurfacePID[3];
     int32_t mcSurfacePIDOutput;
 
-    int32_t fwAltPID[3];
+    int32_t fwAltPID[3];  // CR97
     int32_t fwAltPIDOutput;
     int32_t fwPosPID[3];
     int32_t fwPosPIDOutput;

src/main/cms/cms_menu_navigation.c

@@ -53,6 +53,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
     OSD_SETTING_ENTRY("USE FW YAW CONTROL", SETTING_NAV_USE_FW_YAW_CONTROL),
     OSD_SETTING_ENTRY("LAND SENSITIVITY", SETTING_NAV_LAND_DETECT_SENSITIVITY),
     OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING),
+    OSD_SETTING_ENTRY("MC ACCEL MAX LIMIT", SETTING_NAV_MC_XY_ACCEL_MAX_LIMIT), // CR47
 
     OSD_BACK_AND_END_ENTRY,
  };
@@ -121,7 +122,6 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] =
     OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING),
     OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD),
     OSD_SETTING_ENTRY("MANUAL THR INCREASE", SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE),
-    OSD_SETTING_ENTRY("MC ACCEL MAX LIMIT", SETTING_NAV_MC_XY_ACCEL_MAX_LIMIT), // CR47
 
     OSD_BACK_AND_END_ENTRY,
 };

src/main/fc/settings.yaml

@@ -2003,6 +2003,14 @@ groups:
         field: bank_fw.pid[PID_POS_Z].D
         min: 0
         max: 255
+# CR97
+      # - name: nav_fw_pos_z_ff
+        # description: "Feed forward gain of altitude PID controller (Fixedwing)"
+        # default_value: 10
+        # field: bank_fw.pid[PID_POS_Z].FF
+        # min: 0
+        # max: 255
+# CR97
       - name: nav_fw_pos_xy_p
         description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH"
         default_value: 75

src/main/navigation/navigation.c

@@ -3049,30 +3049,42 @@ bool isProbablyStillFlying(void)
     // lastUpdateTimeUs = currentTimeUs;
 // }
 // CR97
-bool isAltitudeLimitExceeded(void)
+static bool isMaxAltitudeLimitExceeded(void)
 {
     return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
 }
 
-int32_t getClimbRate(float targetAltitude)
+int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
-    if (navConfig()->general.max_altitude && isAltitudeLimitExceeded()) {
+    if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
         targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude);
     }
 
-    const int32_t targetAltitudeError = navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude;
-    const float maxClimbRate = posControl.flags.isAdjustingAltitude ? navConfig()->general.max_manual_climb_rate : navConfig()->general.max_auto_climb_rate;
+    float targetVel = 0.0f;
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
-    const float minClimbRate = emergLandingIsActive ? 100.0f : 0.0f;
-    const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? maxClimbRate * 5 : maxClimbRate;
-    const int8_t direction = targetAltitudeError < 0 ? 1 : -1;
 
-    const float verticalVelScaled = scaleRangef(targetAltitudeError, 0.0f, -maxRateCutoffAlt * direction, minClimbRate, maxClimbRate);
+    float maxClimbRate = navConfig()->general.max_auto_climb_rate;
+    if (emergLandingIsActive) {
+        maxClimbRate = navConfig()->general.emerg_descent_rate;
+    } else if (posControl.flags.isAdjustingAltitude) {
+        maxClimbRate = navConfig()->general.max_manual_climb_rate;
+    }
+    const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
+
+    if (STATE(MULTIROTOR)) {
+        targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
+    } else {
+        // const uint16_t maxRateCutoffAlt = maxClimbRate * 5;
+        // const int8_t direction = targetAltitudeError < 0 ? 1 : -1;
+
+        // targetVel = direction * scaleRangef(targetAltitudeError, 0.0f, -maxRateCutoffAlt * direction, 0.0f, maxClimbRate);
+        targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate);
+    }
 
-    if (emergLandingIsActive && targetAltitudeError < 50) {
+    if (emergLandingIsActive && targetAltitudeError > -50) {
         return -100;
     } else {
-        return direction * constrainf(verticalVelScaled, minClimbRate, maxClimbRate);
+        return constrainf(targetVel, -maxClimbRate, maxClimbRate);
     }
 }
 
@@ -3098,7 +3110,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
      * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
      * In other words, when altitude is reached, allow it only to shrink
      */
-    if (navConfig()->general.max_altitude && isAltitudeLimitExceeded()) {
+    if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
     }
 
@@ -4341,6 +4353,7 @@ void navigationUsePIDs(void)
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,  // CR97 uses 100, original was 10
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,  // CR97 uses 100, original was 10
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,  // CR97 uses 100, original was 10
+                                        // (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,  // CR97 new FF, original was 0.0f
                                         0.0f,
                                         NAV_DTERM_CUT_HZ,
                                         0.0f

src/main/navigation/navigation_fixedwing.c

@@ -139,7 +139,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     float desiredClimbRate = posControl.desiredState.vel.z;
 
     if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
-        desiredClimbRate = getClimbRate(posControl.desiredState.pos.z);
+        desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
     }
 
     // CR96
@@ -165,6 +165,7 @@ DEBUG_SET(DEBUG_ALWAYS, 2, desiredClimbRate - currentClimbRate);
     // Apply low-pass filter to prevent rapid correction
     targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 DEBUG_SET(DEBUG_ALWAYS, 3, targetPitchAngle);
+DEBUG_SET(DEBUG_ALWAYS, 4, posControl.desiredState.pos.z);
     // Reconstrain pitch angle ( >0 - climb, <0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
     posControl.rcAdjustment[PITCH] = targetPitchAngle;

src/main/navigation/navigation_multicopter.c

@@ -62,13 +62,24 @@ static int16_t altHoldThrottleRCZero = 1500;
 static pt1Filter_t altholdThrottleFilterState;
 static bool prepareForTakeoffOnReset = false;
 static sqrt_controller_t alt_hold_sqrt_controller;
-
+//CR97
+float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
+{
+    return sqrtControllerApply(
+            &alt_hold_sqrt_controller,
+            targetAltitude,
+            navGetCurrentActualPositionAndVelocity()->pos.z,
+            US2S(deltaMicros)
+    );
+}
+// CR97
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
-{
-    float targetVel = posControl.desiredState.vel.z;    // CR97
+{// CR97
+    float targetVel = posControl.desiredState.vel.z;    
+
     if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
-        targetVel = getClimbRate(posControl.desiredState.pos.z);
+        targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
     }
 
     // float targetVel = sqrtControllerApply(
@@ -78,17 +89,17 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
         // US2S(deltaMicros)
     // );
 
-    // hard limit desired target velocity to max_climb_rate
-    float vel_max_z = 0.0f;
+    // // hard limit desired target velocity to max_climb_rate
+    // float vel_max_z = 0.0f;
 
-    if (posControl.flags.isAdjustingAltitude) {
-        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);
+    // if (posControl.flags.isAdjustingAltitude) {
+        // 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);
+    // CR97
     posControl.pids.pos[Z].output_constrained = targetVel;  // only used for Blackbox and OSD info  CR97
 
     // Limit max up/down acceleration target

src/main/navigation/navigation_private.h

@@ -474,7 +474,7 @@ bool isWaypointNavTrackingActive(void);
 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
-int32_t getClimbRate(float targetAltitude);  // CR97
+int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);  // CR97
 
 bool checkForPositionSensorTimeout(void);
 
@@ -493,10 +493,9 @@ bool adjustMulticopterHeadingFromRCInput(void);
 bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
-
 bool isMulticopterLandingDetected(void);
-
 void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
+float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros); // CR97
 
 /* Fixed-wing specific functions */
 void setupFixedWingAltitudeController(void);