Skip to content

Commit 9e8bd55

Browse files
MrD-RCMrD-RC
authored
Merge pull request #10486 from breadoven/abo_failsafe_fixes
2 parents ffd3f70 + 463ba1a commit 9e8bd55

File tree

3 files changed

+22
-18
lines changed

3 files changed

+22
-18
lines changed

‎src/main/flight/failsafe.c

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -535,6 +535,7 @@ void failsafeUpdateState(void)
535535
abortForcedRTH();
536536
failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING);
537537
failsafeActivate(FAILSAFE_LANDING);
538+
activateForcedEmergLanding();
538539
reprocessState = true;
539540
break;
540541
}

‎src/main/navigation/navigation.c

Lines changed: 16 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -359,7 +359,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
359359
#ifdef USE_GEOZONE
360360
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState);
361361
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState);
362-
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState);
362+
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState);
363363
#endif
364364

365365
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
@@ -1007,6 +1007,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
10071007
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
10081008
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
10091009
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1010+
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
1011+
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
10101012
}
10111013
},
10121014

@@ -1204,8 +1206,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
12041206
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS,
12051207
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
12061208
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
1207-
}
1208-
},
1209+
}
1210+
},
12091211

12101212
[NAV_STATE_SEND_TO_IN_PROGESS] = {
12111213
.persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES,
@@ -1217,7 +1219,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
12171219
.mwError = MW_NAV_ERROR_NONE,
12181220
.onEvent = {
12191221
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state
1220-
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED,
1222+
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED,
12211223
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
12221224
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
12231225
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@@ -1690,23 +1692,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
16901692
calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
16911693
return NAV_FSM_EVENT_NONE;
16921694
} else if (geozone.avoidInRTHInProgress) {
1693-
if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {
1695+
if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {
16941696
if (geoZoneIsLastRthWaypoint()) {
16951697
// Already at Home?
16961698
fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
16971699
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
16981700
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
16991701
return NAV_FSM_EVENT_SUCCESS;
17001702
}
1701-
1703+
17021704
posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
17031705
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
17041706
} else {
17051707
geozoneAdvanceRthAvoidWaypoint();
17061708
calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
17071709
return NAV_FSM_EVENT_NONE;
17081710
}
1709-
}
1711+
}
17101712
setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
17111713
return NAV_FSM_EVENT_NONE;
17121714
} else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) {
@@ -2566,7 +2568,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
25662568
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState)
25672569
{
25682570
UNUSED(previousState);
2569-
2571+
25702572
if (checkForPositionSensorTimeout()) {
25712573
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
25722574
}
@@ -3606,12 +3608,12 @@ bool isProbablyStillFlying(void)
36063608
* Z-position controller
36073609
*-----------------------------------------------------------*/
36083610
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
3609-
{
3610-
3611+
{
3612+
36113613
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
3612-
3614+
36133615
#ifdef USE_GEOZONE
3614-
if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
3616+
if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
36153617
(geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) {
36163618
return 0.0f;
36173619
}
@@ -4310,7 +4312,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
43104312
posControl.activeWaypointIndex = posControl.startWpIndex;
43114313
// Reset RTH trackback
43124314
resetRthTrackBack();
4313-
4315+
43144316
#ifdef USE_GEOZONE
43154317
posControl.flags.sendToActive = false;
43164318
#endif
@@ -5109,7 +5111,7 @@ void abortSendTo(void)
51095111

51105112
void activateForcedPosHold(void)
51115113
{
5112-
if (!geozone.avoidInRTHInProgress) {
5114+
if (!geozone.avoidInRTHInProgress) {
51135115
abortFixedWingLaunch();
51145116
posControl.flags.forcedPosholdActive = true;
51155117
navProcessFSMEvents(selectNavEventFromBoxModeInput());

‎src/main/navigation/navigation_fw_launch.c

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -260,7 +260,7 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
260260
static timeMs_t wiggleTime = 0;
261261
static timeMs_t wigglesTime = 0;
262262
static int8_t wiggleStageOne = 0;
263-
static uint8_t wiggleCount = 0;
263+
static uint8_t wiggleCount = 0;
264264
const bool isAircraftWithinLaunchAngle = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
265265
const uint8_t wiggleStrength = (navConfig()->fw.launch_wiggle_wake_idle == 1) ? 50 : 40;
266266
int8_t wiggleDirection = 0;
@@ -276,12 +276,12 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
276276
} else if (wiggleStageOne != wiggleDirection) {
277277
wiggleStageOne = 0;
278278
wiggleCount++;
279-
279+
280280
if (wiggleCount == navConfig()->fw.launch_wiggle_wake_idle) {
281281
return true;
282282
}
283283
}
284-
284+
285285
wiggleTime = US2MS(currentTimeUs);
286286
}
287287

@@ -360,7 +360,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(tim
360360
}
361361

362362
if (navConfig()->fw.launch_wiggle_wake_idle == 0 || navConfig()->fw.launch_idle_motor_timer > 0 ) {
363-
return FW_LAUNCH_EVENT_GOTO_DETECTION;
363+
return FW_LAUNCH_EVENT_GOTO_DETECTION;
364364
}
365365

366366
applyThrottleIdleLogic(true);
@@ -613,6 +613,7 @@ uint8_t fixedWingLaunchStatus(void)
613613
void abortFixedWingLaunch(void)
614614
{
615615
setCurrentState(FW_LAUNCH_STATE_ABORTED, 0);
616+
DISABLE_FLIGHT_MODE(NAV_LAUNCH_MODE);
616617
}
617618

618619
const char * fixedWingLaunchStateMessage(void)

0 commit comments

Comments
 (0)