diff --git a/docs/Settings.md b/docs/Settings.md index 73ac0406517..e8e419e4964 100644 --- a/docs/Settings.md +++ b/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 | --- diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 5fd9df7d973..1f8eb9bb55e 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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), diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 27b88800841..47c359b00c2 100644 --- a/src/main/fc/rc_adjustments.c +++ b/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()? diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index d5119ae927e..d900dc82475 100644 --- a/src/main/fc/rc_adjustments.h +++ b/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; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c875944a694..2ee8df4cc89 100644 --- a/src/main/fc/settings.yaml +++ b/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,7 +4396,7 @@ 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 @@ -4398,7 +4404,7 @@ groups: - 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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f1b2f567812..ae8eb57fb30 100644 --- a/src/main/flight/pid.c +++ b/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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 77e5f8a583b..743dc5f83d7 100644 --- a/src/main/io/osd.c +++ b/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,7 +3916,7 @@ 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); @@ -3920,8 +3924,8 @@ static bool osdDrawSingleElement(uint8_t item) 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,7 +6049,7 @@ 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; @@ -6053,7 +6057,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter 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; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index ef718c9cf01..a8e74e4d651 100644 --- a/src/main/io/osd.h +++ b/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; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index c0120c7faa1..fe9c7feeaa8 100644 --- a/src/main/io/osd_dji_hd.c +++ b/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; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a55c6e2e4aa..4ad0868abb7 100644 --- a/src/main/navigation/navigation.c +++ b/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 );