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

Fix a bug in RTH altitude calculation. Bug is observed as RTH never finishing the CLIMB stage

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-24 14:58:21 +10:00
parent 5e2a537d8c
commit e33dda884d
3 changed files with 22 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/flight/navigation_rewrite.c
View file

@ -1098,7 +1098,7 @@ static void updateHomePositionCompatibility(void)
static void updateDesiredRTHAltitude(void)
{
if (ARMING_FLAG(ARMED)) {
if (!(navGetStateFlags(posControl.navState) & NAV_MODE_RTH)) {
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
switch (posControl.navConfig->flags.rth_alt_control_style) {
case NAV_RTH_NO_ALT:
posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z;
@ -1467,7 +1467,7 @@ static void setDesiredPositionToWaypointAndUpdateInitialBearing(navWaypointPosit
* In PH mode our waypoint is hold position */
bool isApproachingLastWaypoint(void)
{
if (navGetStateFlags(posControl.navState) & NAV_MODE_WP) {
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount == 0) {
/* No waypoints, holding current position */
return true;
@ -1848,7 +1848,7 @@ void abortForcedRTH(void)
rthState_e getStateOfForcedRTH(void)
{
if (navGetStateFlags(posControl.navState) & NAV_MODE_RTH) {
if (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) {
if (posControl.navState == NAV_STATE_RTH_2D_FINISHED || posControl.navState == NAV_STATE_RTH_3D_FINISHED) {
return RTH_HAS_LANDED;

11
src/main/flight/navigation_rewrite.h
View file

@ -38,17 +38,6 @@
// Maximum number of waypoints, special waypoint 0 = home,
#define NAV_MAX_WAYPOINTS 15
// navigation mode
typedef enum navigationMode_e {
NAV_MODE_NONE = 0,
NAV_MODE_ALTHOLD,
NAV_MODE_POSHOLD_2D,
NAV_MODE_POSHOLD_3D,
NAV_MODE_WP,
NAV_MODE_RTH,
NAV_MODE_RTH_2D,
} navigationMode_t;
enum {
NAV_GPS_ATTI = 0, // Pitch/roll stick controls attitude (pitch/roll lean angles)
NAV_GPS_CRUISE = 1 // Pitch/roll stick controls velocity (forward/right speed)

38
src/main/flight/navigation_rewrite_private.h
View file

@ -153,29 +153,29 @@ typedef enum {
NAV_STATE_POSHOLD_3D_INITIALIZE, // 6
NAV_STATE_POSHOLD_3D_IN_PROGRESS, // 7
NAV_STATE_RTH_INITIALIZE,
NAV_STATE_RTH_INITIALIZE, // 8
NAV_STATE_RTH_2D_INITIALIZE,
NAV_STATE_RTH_2D_HEAD_HOME,
NAV_STATE_RTH_2D_FINISHING,
NAV_STATE_RTH_2D_FINISHED,
NAV_STATE_RTH_2D_INITIALIZE, // 9
NAV_STATE_RTH_2D_HEAD_HOME, // 10
NAV_STATE_RTH_2D_FINISHING, // 11
NAV_STATE_RTH_2D_FINISHED, // 12
NAV_STATE_RTH_3D_INITIALIZE,
NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
NAV_STATE_RTH_3D_HEAD_HOME,
NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
NAV_STATE_RTH_3D_LANDING,
NAV_STATE_RTH_3D_FINISHING,
NAV_STATE_RTH_3D_FINISHED,
NAV_STATE_RTH_3D_INITIALIZE, // 13
NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // 14
NAV_STATE_RTH_3D_HEAD_HOME, // 15
NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, // 16
NAV_STATE_RTH_3D_LANDING, // 17
NAV_STATE_RTH_3D_FINISHING, // 18
NAV_STATE_RTH_3D_FINISHED, // 19
NAV_STATE_WAYPOINT_INITIALIZE,
NAV_STATE_WAYPOINT_IN_PROGRESS,
NAV_STATE_WAYPOINT_REACHED,
NAV_STATE_WAYPOINT_FINISHED,
NAV_STATE_WAYPOINT_INITIALIZE, // 20
NAV_STATE_WAYPOINT_IN_PROGRESS, // 21
NAV_STATE_WAYPOINT_REACHED, // 22
NAV_STATE_WAYPOINT_FINISHED, // 23
NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
NAV_STATE_EMERGENCY_LANDING_FINISHED,
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 24
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 25
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 26
NAV_STATE_COUNT,
} navigationFSMState_t;