Merge pull request #10541 from breadoven/abo_fw_alt_control_fixes

Fixed wing altitude control fixes

Author: breadoven

docs/Settings.md

@@ -3118,7 +3118,7 @@ Adjusts the deceleration response of fixed wing altitude control as the target a
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 20 | 5 | 100 |
+| 40 | 5 | 100 |
 
 ---
 
@@ -3602,6 +3602,16 @@ D gain of altitude PID controller (Fixedwing)
 
 ---
 
+### nav_fw_pos_z_ff
+
+FF gain of altitude PID controller (Fixedwing)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 0 | 255 |
+
+---
+
 ### nav_fw_pos_z_i
 
 I gain of altitude PID controller (Fixedwing)
@@ -3618,7 +3628,7 @@ P gain of altitude PID controller (Fixedwing)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 40 | 0 | 255 |
+| 30 | 0 | 255 |
 
 ---
 

src/main/cms/cms_menu_imu.c

@@ -227,6 +227,7 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
+    OTHER_PIDFF_ENTRY("ALT FF", &cmsx_pidPosZ.FF),
 
     OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
     OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),

src/main/fc/rc_adjustments.c

@@ -302,6 +302,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
         .adjustmentFunction = ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX,
         .mode = ADJUSTMENT_MODE_STEP,
         .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
     }
 };
 
@@ -545,6 +549,9 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta);
             navigationUsePIDs();
             break;
+        case ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE:
+            applyAdjustmentU8(ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE, &pidProfileMutable()->fwAltControlResponseFactor, delta, SETTING_NAV_FW_ALT_CONTROL_RESPONSE_MIN, SETTING_NAV_FW_ALT_CONTROL_RESPONSE_MAX);
+            break;
         case ADJUSTMENT_HEADING_P:
             applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta);
             // TODO: navigationUsePIDs()?

src/main/fc/rc_adjustments.h

@@ -85,6 +85,7 @@ typedef enum {
     ADJUSTMENT_FW_TPA_TIME_CONSTANT             = 57,
     ADJUSTMENT_FW_LEVEL_TRIM                    = 58,
     ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX       = 59,
+    ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE      = 60,
     ADJUSTMENT_FUNCTION_COUNT // must be last
 } adjustmentFunction_e;
 

src/main/fc/settings.yaml

@@ -2131,7 +2131,7 @@ groups:
         max: 100
       - name: nav_fw_pos_z_p
         description: "P gain of altitude PID controller (Fixedwing)"
-        default_value: 40
+        default_value: 30
         field: bank_fw.pid[PID_POS_Z].P
         min: 0
         max: 255
@@ -2147,9 +2147,15 @@ groups:
         field: bank_fw.pid[PID_POS_Z].D
         min: 0
         max: 255
+      - name: nav_fw_pos_z_ff
+        description: "FF gain of altitude PID controller (Fixedwing)"
+        default_value: 10
+        field: bank_fw.pid[PID_POS_Z].FF
+        min: 0
+        max: 255
       - name: nav_fw_alt_control_response
         description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude."
-        default_value: 20
+        default_value: 40
         field: fwAltControlResponseFactor
         min: 5
         max: 100
@@ -4390,15 +4396,15 @@ groups:
         field: safeAltitudeDistance
         min: 0
         max: 10000
-      - name: geozone_safehome_as_inclusive 
+      - name: geozone_safehome_as_inclusive
         description: "Treat nearest safehome as inclusive geozone"
         type: bool
         field: nearestSafeHomeAsInclusivZone
         default_value: OFF
       - name: geozone_safehome_zone_action
         description:  "Fence action for safehome zone"
         default_value: "NONE"
-        field: safeHomeFenceAction    
+        field: safeHomeFenceAction
         table: fence_action
         type: uint8_t
       - name: geozone_mr_stop_distance
@@ -4412,4 +4418,4 @@ groups:
         default_value: RTH
         field: noWayHomeAction
         table: geozone_rth_no_way_home
-        type: uint8_t    
+        type: uint8_t

src/main/flight/pid.c

@@ -179,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -242,8 +242,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                 [PID_POS_Z] = {
                     .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 100
                     .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 100
-                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 100
-                    .FF = 0,
+                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 200
+                    .FF = SETTING_NAV_FW_POS_Z_FF_DEFAULT,    // FW_POS_Z_FF * 100
                 },
                 [PID_POS_XY] = {
                     .P = SETTING_NAV_FW_POS_XY_P_DEFAULT,     // FW_POS_XY_P * 100
@@ -1206,7 +1206,7 @@ void FAST_CODE pidController(float dT)
 
         // Limit desired rate to something gyro can measure reliably
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
-    
+
 #ifdef USE_ADAPTIVE_FILTER
         adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]);
 #endif

src/main/io/osd.c

@@ -1726,7 +1726,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 tfp_sprintf(buff + 1, "%2d", osdRssi);
             else
                 tfp_sprintf(buff + 1, "%c ", SYM_MAX);
-            
+
             if (osdRssi < osdConfig()->rssi_alarm) {
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
@@ -2393,7 +2393,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #ifdef USE_GEOZONE
             if (FLIGHT_MODE(NAV_SEND_TO))
                 p = "AUTO";
-            else 
+            else
 #endif
             if (FLIGHT_MODE(FAILSAFE_MODE))
                 p = "!FS!";
@@ -2546,7 +2546,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else {
                 tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN);
             }
-                
+
             if (!failsafeIsReceivingRxData()) {
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             } else if (rxLinkStatistics.downlinkLQ < osdConfig()->link_quality_alarm) {
@@ -2608,7 +2608,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 buff[i] = ' ';
         buff[4] = '\0';
         break;
-    
+
     case OSD_RX_MODE:
         displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE);
         strcat(buff, rxLinkStatistics.mode);
@@ -3083,6 +3083,10 @@ static bool osdDrawSingleElement(uint8_t item)
         osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
         return true;
 
+    case OSD_NAV_FW_ALT_CONTROL_RESPONSE:
+        osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "ACR", 0, pidProfile()->fwAltControlResponseFactor, 3, 0, ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE);
+        return true;
+
     case OSD_HEADING_P:
         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P);
         return true;
@@ -3912,16 +3916,16 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
                 int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
                 int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset;
-                osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);            
+                osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);
             } else {
                 if (isGeozoneActive()) {
                     TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
                 }
                 displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr);
             }
         break;
-        }  
-        
+        }
+
         case OSD_H_DIST_TO_FENCE:
         {
             if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) {
@@ -5480,7 +5484,7 @@ static void osdShowSDArmScreen(void)
     displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
     memset(buf, '\0', sizeof(buf));
 #if defined(USE_GPS)
-#if defined (USE_SAFE_HOME) 
+#if defined (USE_SAFE_HOME)
     if (posControl.safehomeState.distance) {
         safehomeRow = armScreenRow;
         armScreenRow += 2;
@@ -6045,15 +6049,15 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 case GEOZONE_MESSAGE_STATE_NFZ:
                     messages[messageCount++] = OSD_MSG_NFZ;
                     break;
-                case GEOZONE_MESSAGE_STATE_LEAVING_FZ:      
+                case GEOZONE_MESSAGE_STATE_LEAVING_FZ:
                     osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
                     tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf);
                     messages[messageCount++] = messageBuf;
                     break;
                 case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ:
                     messages[messageCount++] = OSD_MSG_OUTSIDE_FZ;
                     break;
-                case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:    
+                case GEOZONE_MESSAGE_STATE_ENTERING_NFZ:
                 osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3);
                     if (geozone.zoneInfo == INT32_MAX) {
                         tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M);
@@ -6092,7 +6096,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     if (!geozone.sticksLocked) {
                         messages[messageCount++] = OSD_MSG_MOVE_STICKS;
                     }
-                    break;         
+                    break;
                 case GEOZONE_MESSAGE_STATE_NONE:
                     break;
             }

src/main/io/osd.h

@@ -312,6 +312,7 @@ typedef enum {
     OSD_COURSE_TO_FENCE,
     OSD_H_DIST_TO_FENCE,
     OSD_V_DIST_TO_FENCE,
+    OSD_NAV_FW_ALT_CONTROL_RESPONSE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/io/osd_dji_hd.c

@@ -958,6 +958,9 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
         case ADJUSTMENT_VEL_Z_D:
             tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
             break;
+        case ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE:
+            tfp_sprintf(buff, "ACR %3d", pidProfileMutable()->fwAltControlResponseFactor);
+            break;
         case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
             tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
             break;

src/main/navigation/navigation.c

@@ -4960,8 +4960,8 @@ void navigationUsePIDs(void)
 
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
-                                        0.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
                                         NAV_DTERM_CUT_HZ,
                                         0.0f
     );