diff --git a/docs/Settings.md b/docs/Settings.md index 0e3470a39f..b7b53acab3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -318,9 +318,10 @@ | nav_fw_pos_z_i | 5 | I gain of altitude PID controller (Fixedwing) | | nav_fw_pos_z_p | 40 | P gain of altitude PID controller (Fixedwing) | | nav_fw_yaw_deadband | 0 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | -| nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm] | -| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm] | -| nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] | +| nav_land_maxalt_vspd | 200 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] | +| nav_land_minalt_vspd | 50 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] | +| nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] | +| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] | | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | | nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode | diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index f71385422e..ff9d246ee2 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -75,7 +75,8 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST), OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST), OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING), - OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED), + OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LAND_MINALT_VSPD), + OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LAND_MAXALT_VSPD), OSD_SETTING_ENTRY("LAND SPEED MIN AT", SETTING_NAV_LAND_SLOWDOWN_MINALT), OSD_SETTING_ENTRY("LAND SPEED SLOW AT", SETTING_NAV_LAND_SLOWDOWN_MAXALT), OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE), diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8f968440b5..06f657d922 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1334,7 +1334,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode); sbufWriteU16(dst, navConfig()->general.rth_abort_threshold); sbufWriteU16(dst, navConfig()->general.rth_altitude); - sbufWriteU16(dst, navConfig()->general.land_descent_rate); + sbufWriteU16(dst, navConfig()->general.land_minalt_vspd); + sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd); sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt); sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt); sbufWriteU16(dst, navConfig()->general.emerg_descent_rate); @@ -2307,7 +2308,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src); navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src); navConfigMutable()->general.rth_altitude = sbufReadU16(src); - navConfigMutable()->general.land_descent_rate = sbufReadU16(src); + navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src); + navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src); navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src); navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src); navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4b087bef5d..2e64bf9660 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2278,20 +2278,26 @@ groups: field: general.max_manual_climb_rate min: 10 max: 2000 - - name: nav_landing_speed - description: "Vertical descent velocity during the RTH landing phase. [cm/s]" + - name: nav_land_minalt_vspd + description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]" + default_value: 50 + field: general.land_minalt_vspd + min: 50 + max: 500 + - name: nav_land_maxalt_vspd + description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]" default_value: 200 - field: general.land_descent_rate + field: general.land_maxalt_vspd min: 100 max: 2000 - name: nav_land_slowdown_minalt - description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]" + description: "Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm]" default_value: 500 field: general.land_slowdown_minalt min: 50 max: 1000 - name: nav_land_slowdown_maxalt - description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]" + description: "Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm]" default_value: 2000 field: general.land_slowdown_maxalt min: 500 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 82a50595a9..dd89b8b69e 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -96,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -124,9 +124,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, - .land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT, // centimeters/s .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters + .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s + .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT, // centimeters/s .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters @@ -1413,22 +1414,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. - descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); + descentVelLimited = navConfig()->general.land_minalt_vspd; } else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. - float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt) - / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt + float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z, + navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt, + navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); + descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - // Do not allow descent velocity slower than -50cm/s so the landing detector works. - descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f); } - updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); return NAV_FSM_EVENT_NONE; } @@ -1452,7 +1451,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation UNUSED(previousState); if (STATE(ALTITUDE_CONTROL)) { - updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME + updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME } // Prevent I-terms growing when already landed diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 6b85cdf32c..863f07a4bf 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -208,7 +208,8 @@ typedef struct navConfig_s { uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed uint16_t max_manual_climb_rate; // manual velocity control max vertical speed - uint16_t land_descent_rate; // normal RTH landing descent rate + uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt + uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend uint16_t land_slowdown_maxalt; // Altitude to start lowering descent rate during RTH descend uint16_t emerg_descent_rate; // emergency landing descent rate