1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00

Merge pull request #8686 from breadoven/abo_manual_emerg_landing

Manual emergency landing FW and MR
This commit is contained in:
Paweł Spychalski 2023-01-27 16:11:05 +01:00 committed by GitHub
commit bb79ad8f02
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 86 additions and 11 deletions

View file

@ -346,6 +346,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
}
}
// Inhibit Failsafe if emergency landing triggered manually
if (posControl.flags.manualEmergLandActive) {
return FAILSAFE_PROCEDURE_NONE;
}
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 &&

View file

@ -1774,9 +1774,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
{
// TODO:
UNUSED(previousState);
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
resetPositionController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
}
// Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
// Make sure terrain following is not enabled
resetAltitudeController(false);
@ -1788,6 +1792,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PR
{
UNUSED(previousState);
// Reset target position if too far away for some reason, e.g. GPS recovered since start landing.
if (posControl.flags.estPosStatus >= EST_USABLE) {
float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f;
if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) {
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
}
}
if (STATE(LANDING_DETECTED)) {
return NAV_FSM_EVENT_SUCCESS;
}
@ -3421,7 +3433,10 @@ bool isNavHoldPositionActive(void)
return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
}
// RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE);
// Also hold position during emergency landing if position valid
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
FLIGHT_MODE(NAV_POSHOLD_MODE) ||
navigationIsExecutingAnEmergencyLanding();
}
float getActiveWaypointSpeed(void)
@ -3520,6 +3535,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
// If we are disarmed, abort forced RTH or Emergency Landing
posControl.flags.forcedRTHActivated = false;
posControl.flags.forcedEmergLandingActivated = false;
posControl.flags.manualEmergLandActive = false;
// ensure WP missions always restart from first waypoint after disarm
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
@ -3596,6 +3612,41 @@ static bool isWaypointMissionValid(void)
return posControl.waypointListValid && (posControl.waypointCount > 0);
}
static void checkManualEmergencyLandingControl(void)
{
static timeMs_t timeout = 0;
static int8_t counter = 0;
static bool toggle;
timeMs_t currentTimeMs = millis();
if (timeout && currentTimeMs > timeout) {
timeout += 1000;
counter -= counter ? 1 : 0;
if (!counter) {
timeout = 0;
}
}
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
if (!timeout && toggle) {
timeout = currentTimeMs + 4000;
}
counter += toggle;
toggle = false;
} else {
toggle = true;
}
// Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
if (counter >= 5) {
counter = 0;
posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
if (!posControl.flags.manualEmergLandActive) {
navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
}
}
}
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
{
static bool canActivateWaypoint = false;
@ -3621,8 +3672,12 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
posControl.flags.rthTrackbackActive = isExecutingRTH;
}
/* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */
if (posControl.flags.forcedEmergLandingActivated) {
/* Emergency landing controlled manually by rapid switching of Poshold mode.
* Landing toggled ON or OFF for each Poshold activation sequence */
checkManualEmergencyLandingControl();
/* Emergency landing triggered by failsafe Landing or manually initiated */
if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}

View file

@ -752,8 +752,6 @@ bool isFixedWingLandingDetected(void)
*-----------------------------------------------------------*/
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
{
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
@ -765,6 +763,18 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
} else {
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
}
if (posControl.flags.estPosStatus >= EST_USABLE) { // Hold position if possible
applyFixedWingPositionController(currentTimeUs);
int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL],
-DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle),
DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = 0;
} else {
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
}
}
/*-----------------------------------------------------------

View file

@ -814,8 +814,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
static timeUs_t previousTimePositionUpdate = 0;
/* Attempt to stabilise */
rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0;
rcCommand[YAW] = 0;
if ((posControl.flags.estAltStatus < EST_USABLE)) {
@ -852,6 +850,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Update throttle controller
rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
// Hold position if possible
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
applyMulticopterPositionController(currentTimeUs);
} else {
rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0;
}
}
/*-----------------------------------------------------------

View file

@ -104,14 +104,13 @@ typedef struct navigationFlags_s {
bool forcedRTHActivated;
bool forcedEmergLandingActivated;
bool wpMissionPlannerActive; // Activation status of WP mission planner
/* Landing detector */
bool resetLandingDetector;
bool wpMissionPlannerActive; // Activation status of WP mission planner
bool rthTrackbackActive; // Activation status of RTH trackback
bool wpTurnSmoothingActive; // Activation status WP turn smoothing
bool manualEmergLandActive; // Activation status of manual emergency landing
} navigationFlags_t;
typedef struct {