Skip to content

Commit

Permalink
Merge pull request #10486 from breadoven/abo_failsafe_fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@MrD-RC
MrD-RC authored Dec 5, 2024
2 parents ffd3f70 + 463ba1a commit 9e8bd55
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 18 deletions.
1 change: 1 addition & 0 deletions src/main/flight/failsafe.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -535,6 +535,7 @@ void failsafeUpdateState(void)
abortForcedRTH();
failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING);
failsafeActivate(FAILSAFE_LANDING);
activateForcedEmergLanding();
reprocessState = true;
break;
}
Expand Down
30 changes: 16 additions & 14 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -359,7 +359,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
#ifdef USE_GEOZONE
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState);
#endif

static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
Expand Down Expand Up @@ -1007,6 +1007,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},

Expand Down Expand Up @@ -1204,8 +1206,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
}
},

[NAV_STATE_SEND_TO_IN_PROGESS] = {
.persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES,
Expand All @@ -1217,7 +1219,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
Expand Down Expand Up @@ -1690,23 +1692,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
return NAV_FSM_EVENT_NONE;
} else if (geozone.avoidInRTHInProgress) {
if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {
if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) {
if (geoZoneIsLastRthWaypoint()) {
// Already at Home?
fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS;
}

posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
} else {
geozoneAdvanceRthAvoidWaypoint();
calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint());
return NAV_FSM_EVENT_NONE;
}
}
}
setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE;
} else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) {
Expand Down Expand Up @@ -2566,7 +2568,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState)
{
UNUSED(previousState);

if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
Expand Down Expand Up @@ -3606,12 +3608,12 @@ bool isProbablyStillFlying(void)
* Z-position controller
*-----------------------------------------------------------*/
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
{
{

const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();

#ifdef USE_GEOZONE
if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) ||
(geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) {
return 0.0f;
}
Expand Down Expand Up @@ -4310,7 +4312,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
resetRthTrackBack();

#ifdef USE_GEOZONE
posControl.flags.sendToActive = false;
#endif
Expand Down Expand Up @@ -5109,7 +5111,7 @@ void abortSendTo(void)

void activateForcedPosHold(void)
{
if (!geozone.avoidInRTHInProgress) {
if (!geozone.avoidInRTHInProgress) {
abortFixedWingLaunch();
posControl.flags.forcedPosholdActive = true;
navProcessFSMEvents(selectNavEventFromBoxModeInput());
Expand Down
9 changes: 5 additions & 4 deletions src/main/navigation/navigation_fw_launch.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -260,7 +260,7 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
static timeMs_t wiggleTime = 0;
static timeMs_t wigglesTime = 0;
static int8_t wiggleStageOne = 0;
static uint8_t wiggleCount = 0;
static uint8_t wiggleCount = 0;
const bool isAircraftWithinLaunchAngle = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
const uint8_t wiggleStrength = (navConfig()->fw.launch_wiggle_wake_idle == 1) ? 50 : 40;
int8_t wiggleDirection = 0;
Expand All @@ -276,12 +276,12 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
} else if (wiggleStageOne != wiggleDirection) {
wiggleStageOne = 0;
wiggleCount++;

if (wiggleCount == navConfig()->fw.launch_wiggle_wake_idle) {
return true;
}
}

wiggleTime = US2MS(currentTimeUs);
}

Expand Down Expand Up @@ -360,7 +360,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(tim
}

if (navConfig()->fw.launch_wiggle_wake_idle == 0 || navConfig()->fw.launch_idle_motor_timer > 0 ) {
return FW_LAUNCH_EVENT_GOTO_DETECTION;
return FW_LAUNCH_EVENT_GOTO_DETECTION;
}

applyThrottleIdleLogic(true);
Expand Down Expand Up @@ -613,6 +613,7 @@ uint8_t fixedWingLaunchStatus(void)
void abortFixedWingLaunch(void)
{
setCurrentState(FW_LAUNCH_STATE_ABORTED, 0);
DISABLE_FLIGHT_MODE(NAV_LAUNCH_MODE);
}

const char * fixedWingLaunchStateMessage(void)
Expand Down

0 comments on commit 9e8bd55

Please sign in to comment.