Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Fix failsafe & navigation clash

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-07-08 21:46:21 +10:00
parent a1f34965de
commit 9baede1ab7
3 changed files with 16 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/flight/failsafe.c
View file

@ -83,12 +83,14 @@ typedef enum {
} failsafeChannelBehavior_e;
typedef struct {
bool bypassNavigation;
bool forceAngleMode;
failsafeChannelBehavior_e channelBehavior[4];
} failsafeProcedureLogic_t;
static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
[FAILSAFE_PROCEDURE_AUTO_LANDING] = {
.bypassNavigation = true,
.forceAngleMode = true,
.channelBehavior = {
FAILSAFE_CHANNEL_AUTO, // ROLL
@ -99,6 +101,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
},
[FAILSAFE_PROCEDURE_DROP_IT] = {
.bypassNavigation = true,
.forceAngleMode = true,
.channelBehavior = {
FAILFAFE_CHANNEL_NEUTRAL, // ROLL
@ -109,6 +112,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
},
[FAILSAFE_PROCEDURE_RTH] = {
.bypassNavigation = false,
.forceAngleMode = true,
.channelBehavior = {
FAILFAFE_CHANNEL_NEUTRAL, // ROLL
@ -119,6 +123,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
},
[FAILSAFE_PROCEDURE_NONE] = {
.bypassNavigation = false,
.forceAngleMode = false,
.channelBehavior = {
FAILSAFE_CHANNEL_HOLD, // ROLL
@ -160,6 +165,11 @@ void failsafeInit(void)
}
#ifdef NAV
bool failsafeBypassNavigation(void)
{
return failsafeState.active && failsafeState.controlling && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation;
}
bool failsafeMayRequireNavigationMode(void)
{
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH;

1
src/main/flight/failsafe.h
View file

@ -112,6 +112,7 @@ void failsafeApplyControlInput(void);
bool failsafeRequiresAngleMode(void);
bool failsafeRequiresMotorStop(void);
bool failsafeShouldApplyControlInput(void);
bool failsafeBypassNavigation(void);
void failsafeUpdateRcCommandValues(void);
void failsafeOnValidDataReceived(void);

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

@ -2342,6 +2342,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
//We can switch modes only when ARMED
if (ARMING_FLAG(ARMED)) {
// Ask failsafe system if we can use navigation system
if (failsafeBypassNavigation()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode();