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

Merge pull request #1873 from iNavFlight/de_failsafe_nav_fix

Browse source 
Fix failsafe & navigation clash
This commit is contained in:
Konstantin Sharlaimov 2017-11-30 21:19:14 +10:00 • committed by GitHub
commit b9fe92e8a9
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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

@ -88,12 +88,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
@ -104,6 +106,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
},
[FAILSAFE_PROCEDURE_DROP_IT] = {
.bypassNavigation = true,
.forceAngleMode = true,
.channelBehavior = {
FAILSAFE_CHANNEL_NEUTRAL, // ROLL
@ -114,6 +117,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
},
[FAILSAFE_PROCEDURE_RTH] = {
.bypassNavigation = false,
.forceAngleMode = true,
.channelBehavior = {
FAILSAFE_CHANNEL_NEUTRAL, // ROLL
@ -124,6 +128,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
},
[FAILSAFE_PROCEDURE_NONE] = {
.bypassNavigation = false,
.forceAngleMode = false,
.channelBehavior = {
FAILSAFE_CHANNEL_HOLD, // ROLL
@ -164,6 +169,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

@ -164,6 +164,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

@ -2326,6 +2326,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();