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

Merge pull request #1873 from iNavFlight/de_failsafe_nav_fix

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

View file

@ -88,12 +88,14 @@ typedef enum {
} failsafeChannelBehavior_e; } failsafeChannelBehavior_e;
typedef struct { typedef struct {
bool bypassNavigation;
bool forceAngleMode; bool forceAngleMode;
failsafeChannelBehavior_e channelBehavior[4]; failsafeChannelBehavior_e channelBehavior[4];
} failsafeProcedureLogic_t; } failsafeProcedureLogic_t;
static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
[FAILSAFE_PROCEDURE_AUTO_LANDING] = { [FAILSAFE_PROCEDURE_AUTO_LANDING] = {
.bypassNavigation = true,
.forceAngleMode = true, .forceAngleMode = true,
.channelBehavior = { .channelBehavior = {
FAILSAFE_CHANNEL_AUTO, // ROLL FAILSAFE_CHANNEL_AUTO, // ROLL
@ -104,6 +106,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
}, },
[FAILSAFE_PROCEDURE_DROP_IT] = { [FAILSAFE_PROCEDURE_DROP_IT] = {
.bypassNavigation = true,
.forceAngleMode = true, .forceAngleMode = true,
.channelBehavior = { .channelBehavior = {
FAILSAFE_CHANNEL_NEUTRAL, // ROLL FAILSAFE_CHANNEL_NEUTRAL, // ROLL
@ -114,6 +117,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
}, },
[FAILSAFE_PROCEDURE_RTH] = { [FAILSAFE_PROCEDURE_RTH] = {
.bypassNavigation = false,
.forceAngleMode = true, .forceAngleMode = true,
.channelBehavior = { .channelBehavior = {
FAILSAFE_CHANNEL_NEUTRAL, // ROLL FAILSAFE_CHANNEL_NEUTRAL, // ROLL
@ -124,6 +128,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
}, },
[FAILSAFE_PROCEDURE_NONE] = { [FAILSAFE_PROCEDURE_NONE] = {
.bypassNavigation = false,
.forceAngleMode = false, .forceAngleMode = false,
.channelBehavior = { .channelBehavior = {
FAILSAFE_CHANNEL_HOLD, // ROLL FAILSAFE_CHANNEL_HOLD, // ROLL
@ -164,6 +169,11 @@ void failsafeInit(void)
} }
#ifdef NAV #ifdef NAV
bool failsafeBypassNavigation(void)
{
return failsafeState.active && failsafeState.controlling && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation;
}
bool failsafeMayRequireNavigationMode(void) bool failsafeMayRequireNavigationMode(void)
{ {
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH; return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH;

View file

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

View file

@ -2326,6 +2326,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
//We can switch modes only when ARMED //We can switch modes only when ARMED
if (ARMING_FLAG(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) // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
bool canActivateAltHold = canActivateAltHoldMode(); bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode(); bool canActivatePosHold = canActivatePosHoldMode();