rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-1122 safer sanity checks and arming prevention in case of invalid flight plan

Browse Source
This commit is contained in:
Corvus Corax 2014-01-12 21:37:40 +01:00
parent 3d5e27f5f2
commit 76ee48bc44
2 changed files with 26 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/libraries/sanitycheck.c
View File

@ -35,6 +35,7 @@
// UAVOs
#include <manualcontrolsettings.h>
#include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h>
/****************************
@ -151,9 +152,15 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports PathPlanner
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:

22
flight/modules/PathPlanner/pathplanner.c
View File

@ -135,16 +135,20 @@ static void pathPlannerTask()
// check flight plan validity early to raise alarm
// even if not in guided mode
uint8_t validFlightPlan = checkFlightPlan();
if (!validFlightPlan) {
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
}
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
if (!validFlightPlan) {
// unverified flight plans are only a warning while we are not in pathplanner mode
// so it does not prevent arming. However manualcontrols safety check
// shall test for this warning when pathplan is on the flight mode selector
// thus a valid flight plan is a prerequirement for arming
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
}
return;
}
@ -156,7 +160,9 @@ static void pathPlannerTask()
if (!validFlightPlan) {
// At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
// Failsafe: behave as if in return to base mode
// what to do in this case is debatable, it might be better to just make a forced disarming but rth has a higher chance of survival when in flight.
// what to do in this case is debatable, it might be better to just
// make a forced disarming but rth has a higher chance of survival when
// in flight.
pathplanner_active = false;
if (!failsafeRTHset) {
@ -178,8 +184,12 @@ static void pathPlannerTask()
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
return;
}
failsafeRTHset = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
WaypointActiveGet(&waypointActive);