From 55113eea6547c3d3da4c99dd09feeecea58805df Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 19 Jan 2021 13:35:16 +0000 Subject: [PATCH] Setting Change Added Spiral setting to nav_rth_climb_first setting. --- docs/Settings.md | 3 +-- src/main/cms/cms_menu_navigation.c | 1 - src/main/fc/settings.yaml | 12 +++++------- src/main/navigation/navigation.c | 7 +++---- src/main/navigation/navigation.h | 7 ++++++- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 1f91251f74..8e7c689039 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -330,9 +330,8 @@ | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | -| nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | +| nav_rth_climb_first | ON | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) | | nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | -| nav_rth_fw_spiral_climb | OFF | Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius. | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | | nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 4b5bd24686..d17d3df290 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -73,7 +73,6 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("RTH ALT MODE", SETTING_NAV_RTH_ALT_MODE), OSD_SETTING_ENTRY("RTH ALT", SETTING_NAV_RTH_ALTITUDE), OSD_SETTING_ENTRY("CLIMB BEFORE RTH", SETTING_NAV_RTH_CLIMB_FIRST), - OSD_SETTING_ENTRY("FW SPIRAL CLIMB", SETTING_NAV_RTH_FW_SPIRAL_CLIMB), 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), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a33219eaa4..bcecc1e3b8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -155,6 +155,9 @@ tables: - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] + - name: nav_rth_climb_first + enum: navRTHClimbFirst_e + values: ["OFF", "ON", "ON_FW_SPIRAL"] groups: - name: PG_GYRO_CONFIG @@ -2112,10 +2115,10 @@ groups: field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop - name: nav_rth_climb_first - description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way." + description: "If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor)" default_value: "ON" field: general.flags.rth_climb_first - type: bool + table: nav_rth_climb_first - name: nav_rth_climb_ignore_emerg description: "If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status." default_value: "OFF" @@ -2136,11 +2139,6 @@ groups: default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode - - name: nav_rth_fw_spiral_climb - description: "Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius." - default_value: "OFF" - field: general.flags.rth_fw_spiral_climb - type: bool - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: "50000" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 69bf9dc03e..e6086c4064 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -106,7 +106,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .rth_fw_spiral_climb = 0, // Loiter climb for FW RTH .nav_overrides_motor_stop = NOMS_ALL_NAV, }, @@ -1152,7 +1151,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati if (STATE(FIXED_WING_LEGACY)) { // Airplane - climbout before heading home - if (navConfig()->general.flags.rth_fw_spiral_climb) { + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { // Spiral climb centered at xy of RTH activation calculateInitialHoldPosition(&targetHoldPos); } else { @@ -1201,7 +1200,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1232,7 +1231,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // Climb to safe altitude and turn to correct direction if (STATE(FIXED_WING_LEGACY)) { - if (navConfig()->general.flags.rth_fw_spiral_climb) { + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); } else { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 445d2d598d..29914f5877 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -131,6 +131,12 @@ typedef enum { NOMS_ALL_NAV } navOverridesMotorStop_e; +typedef enum { + OFF, + ON, + ON_FW_SPIRAL, +} navRTHClimbFirst_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -183,7 +189,6 @@ typedef struct navConfig_s { uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor - uint8_t rth_fw_spiral_climb; // Enable RTH spiral climb for FW when "climb first" selected } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)