update

breadoven · Mar 10, 2024 · 93f98a0 · 93f98a0
1 parent ef7c92f
commit 93f98a0
Show file tree

Hide file tree

Showing 7 changed files with 54 additions and 29 deletions.
diff --git a/docs/Settings.md b/docs/Settings.md
@@ -2862,6 +2862,16 @@ Adjusts the deceleration response of fixed wing altitude control as the target a
 
 ---
 
+### nav_fw_auto_climb_rate
+
+Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 10 | 2000 |
+
+---
+
 ### nav_fw_bank_angle
 
 Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
@@ -198,6 +198,8 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
 {
     OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),
 
+    OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE),
+
     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -1113,7 +1113,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             legacyLedConfig |= ledConfig->led_function << shiftCount;
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
-            shiftCount += 6; 
+            shiftCount += 6;
             legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
@@ -1340,9 +1340,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_NAV_POSHOLD:
         sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
         sbufWriteU16(dst, navConfig()->general.max_auto_speed);
-        sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate);
+        sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate);
         sbufWriteU16(dst, navConfig()->general.max_manual_speed);
-        sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate);
+        sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_manual_climb_rate : navConfig()->mc.max_manual_climb_rate);
         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
         sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
         sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
@@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                 sbufWriteU8(dst, timerHardware[i].usageFlags);
             }
         break;
-    
+
     case MSP2_INAV_MC_BRAKING:
 #ifdef USE_MR_BRAKING_MODE
         sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
@@ -2394,12 +2394,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         if (dataSize == 13) {
             navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
             navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
-            navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
+            if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
+                navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src);
+            } else {
+                navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
+            }
             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
-            if (mixerConfig()->platformType != PLATFORM_AIRPLANE) {
-                navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
-            }else{
+            if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
                 navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src);
+            } else {
+                navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
             }
             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
             navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
@@ -2729,7 +2733,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_WP:
         if (dataSize == 21) {
-            
+
             const uint8_t msp_wp_no = sbufReadU8(src);     // get the waypoint number
             navWaypoint_t msp_wp;
             msp_wp.action = sbufReadU8(src);    // action
@@ -2943,7 +2947,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             ledConfig->led_position = legacyConfig & 0xFF;
             ledConfig->led_function = (legacyConfig >> 8) & 0xF;
             ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
-            ledConfig->led_color = (legacyConfig >> 18) & 0xF; 
+            ledConfig->led_color = (legacyConfig >> 18) & 0xF;
             ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
             ledConfig->led_params = (legacyConfig >> 28) & 0xF;
 
@@ -3233,7 +3237,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
             fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
             fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);
-             
+
             int16_t head1 = 0, head2 = 0;
             if (sbufReadI16Safe(&head1, src)) {
                 fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
@@ -3283,12 +3287,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src);
             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src);
             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src);
-            
+
         } else {
             return MSP_RESULT_ERROR;
         }
 
-        break;    
+        break;
 
 #endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
@@ -3582,7 +3586,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 	static uint8_t osdPos_x = 0;
 
     //indicate new format hitl 1.4.0
-	sbufWriteU8(dst, 255);  
+	sbufWriteU8(dst, 255);
 
 	if (isOSDTypeSupportedBySimulator())
 	{
@@ -3788,7 +3792,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
         tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
-        
+
         // Check the MSP version of simulator
         if (tmp_u8 != SIMULATOR_MSP_VERSION) {
             break;
@@ -3883,15 +3887,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                 } else {
                     sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
                 }
-                
+
                 // Get the acceleration in 1G units
                 acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
                 acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
                 acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
                 acc.accVibeSq[X] = 0.0f;
                 acc.accVibeSq[Y] = 0.0f;
                 acc.accVibeSq[Z] = 0.0f;
-                
+
                 // Get the angular velocity in DPS
                 gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
                 gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
@@ -3922,7 +3926,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                     simulatorData.airSpeed = sbufReadU16(src);
                 } else {
                     if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
-                        sbufReadU16(src); 
+                        sbufReadU16(src);
                     }
                 }
 
@@ -3997,8 +4001,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             *ret = MSP_RESULT_ERROR;
         }
         break;
-#endif 
-    
+#endif
+
     default:
         // Not handled
         return false;

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2334,7 +2334,7 @@ groups:
         field: w_z_surface_p
         min: 0
         max: 100
-        default_value: 3.5 
+        default_value: 3.5
       - name: inav_w_z_surface_v
         field: w_z_surface_v
         min: 0
@@ -2801,6 +2801,12 @@ groups:
         field: fw.max_bank_angle
         min: 5
         max: 80
+      - name: nav_fw_auto_climb_rate
+        description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
+        default_value: 500
+        field: fw.max_auto_climb_rate
+        min: 10
+        max: 2000
       - name: nav_fw_manual_climb_rate
         description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
         default_value: 300
@@ -4143,7 +4149,7 @@ groups:
     type: navFwAutolandConfig_t
     headers: ["navigation/navigation.h"]
     condition: USE_FW_AUTOLAND
-    members: 
+    members:
       - name: nav_fw_land_approach_length
         description: "Length of the final approach"
         default_value: 35000

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -204,6 +204,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     // Fixed wing
     .fw = {
         .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT,                    // degrees
+        .max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT,          // 5 m/s
         .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT,      // 3 m/s
         .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT,                  // degrees
         .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT,                    // degrees
@@ -1891,7 +1892,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                                     (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
                     }
 
-                    if (fabsf(climbRate) >= navConfig()->general.max_auto_climb_rate) {
+                    if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) {
                         climbRate = 0;
                     }
                     updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
@@ -2212,7 +2213,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
     }
 
     if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
-        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
+        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
         posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER;
         return NAV_FSM_EVENT_SUCCESS;
     }
@@ -3379,13 +3380,13 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
 
-    float maxClimbRate = navConfig()->general.max_auto_climb_rate;
+    float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
     if (posControl.desiredState.climbRateDemand) {
         maxClimbRate = ABS(posControl.desiredState.climbRateDemand);
     } else if (emergLandingIsActive) {
         maxClimbRate = navConfig()->general.emerg_descent_rate;
     } else if (posControl.flags.isAdjustingAltitude) {
-        maxClimbRate = navConfig()->general.max_manual_climb_rate;
+        maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
     }
 
     const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -265,7 +265,7 @@ typedef struct positionEstimationConfig_s {
 
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
-#endif    
+#endif
 } positionEstimationConfig_t;
 
 PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);
@@ -329,7 +329,7 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;                // multicopter max banking angle (deg)
-        uint16_t max_auto_climb_rate;           // max vertical speed limitation cm/sec
+        uint16_t max_auto_climb_rate;           // max vertical speed limitation nav modes cm/sec
         uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed
 
 #ifdef USE_MR_BRAKING_MODE
@@ -351,6 +351,7 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;             // Fixed wing max banking angle (deg)
+        uint16_t max_auto_climb_rate;        // max vertical speed limitation nav modes cm/sec
         uint16_t max_manual_climb_rate;      // manual velocity control max vertical speed
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -288,7 +288,7 @@ typedef enum {
     NAV_STATE_CRUISE_INITIALIZE,
     NAV_STATE_CRUISE_IN_PROGRESS,
     NAV_STATE_CRUISE_ADJUSTING,
-    
+
     NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
     NAV_STATE_FW_LANDING_LOITER,
     NAV_STATE_FW_LANDING_APPROACH,
@@ -462,6 +462,7 @@ typedef struct {
 #endif
     navWaypointPosition_t       activeWaypoint;             // Local position, current bearing and turn angle to next WP, filled on waypoint activation
     int8_t                      activeWaypointIndex;
+    float                       wpInitialAltitude;          // Altitude at start of WP
     float                       wpInitialDistance;          // Distance when starting flight to WP
     float                       wpDistance;                 // Distance to active WP
     timeMs_t                    wpReachedTime;              // Time the waypoint was reached