Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

Changes to fix EXTRA mode

Browse source 
Initial testing went pretty well. Using the RTH climb stage worked perfectly with the AT_LEAST method. However the EXTRA method didn't work. After reviewing the code, I can see why, and have implemented a fix. This should now work as expected. I'll test ASAP.
This commit is contained in:
Darren Lines 2021-07-24 19:55:24 +01:00
parent a1c05beeed
commit 9ed0b1d29b
3 changed files with 10 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

7
docs/Navigation.md
View file

@ -36,17 +36,18 @@ PID meaning:
## NAV RTH - return to home mode
Home for RTH is the position where vehicle was armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. RTH requires accelerometer, compass and GPS sensors.
Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors.
If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
If barometer is present, RTH will maintain altitude during the return and when home is reached copter will attempt automated landing.
When deciding what altitude to maintain, RTH has 4 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):
If barometer is present, RTH will maintain altitude during the return and when home is reached. A copter will attempt automated landing.
When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):
* 0 (NAV_RTH_NO_ALT) - keep current altitude during whole RTH sequence (*nav_rth_altitude* is ignored)
* 1 (NAV_RTH_EXTRA_ALT) - climb to current altitude plus extra margin prior to heading home (*nav_rth_altitude* defines the extra altitude (cm))
* 2 (NAV_RTH_CONST_ALT) - climb/descend to predefined altitude before heading home (*nav_rth_altitude* defined altitude above launch point (cm))
* 3 (NAV_RTH_MAX_ALT) - track maximum altitude of the whole flight, climb to that altitude prior to the return (*nav_rth_altitude* is ignored)
* 4 (NAV_RTH_AT_LEAST_ALT) - same as 2 (NAV_RTH_CONST_ALT), but only climb, do not descend
* 5 (NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) - Same as 4 (NAV_RTH_AT_LEAST_ALT). But, if above the RTH Altitude, the aircraft will gradually descend to the RTH Altitude. The target is to reach the RTH Altitude as it arrives at the home point. This is to save energy during the RTH.
## CLI command `wp` to manage waypoints

5
src/main/navigation/navigation.c
View file

@ -2158,6 +2158,8 @@ static void updateDesiredRTHAltitude(void)
if (ARMING_FLAG(ARMED)) {
if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
|| ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
posControl.rthState.rthStartAltitude = posControl.actualState.abs.pos.z;
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
@ -2191,6 +2193,7 @@ static void updateDesiredRTHAltitude(void)
}
}
} else {
posControl.rthState.rthStartAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
}
@ -2468,7 +2471,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
}
break;
case NAV_RTH_CLIMB_STAGE_EXTRA:
if (posControl.actualState.abs.pos.z >= (posControl.rthState.rthInitialAltitude + navConfig()->general.rth_climb_first_stage_altitude)) {
if (posControl.actualState.abs.pos.z >= (posControl.rthState.rthStartAltitude + navConfig()->general.rth_climb_first_stage_altitude)) {
return true;
}
break;

3
src/main/navigation/navigation_private.h
View file

@ -305,7 +305,8 @@ typedef struct {
typedef struct {
navigationHomeFlags_t homeFlags;
navWaypointPosition_t homePosition; // Original home position and base altitude
float rthInitialAltitude; // Altitude at start of RTH
float rthStartAltitude; // Altitude at start of RTH
float rthInitialAltitude; // Altitude at start of RTH, can include added margins and extra height
float rthFinalAltitude; // Altitude at end of RTH approach
float rthInitialDistance; // Distance when starting flight home
fpVector3_t homeTmpWaypoint; // Temporary storage for home target