rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +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

11
flight/libraries/sanitycheck.c
View File

@ -35,6 +35,7 @@
// UAVOs // UAVOs
#include <manualcontrolsettings.h> #include <manualcontrolsettings.h>
#include <systemsettings.h> #include <systemsettings.h>
#include <systemalarms.h>
#include <taskinfo.h> #include <taskinfo.h>
/**************************** /****************************
@ -151,10 +152,16 @@ int32_t configuration_check()
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) { if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR; severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { } else {
// Revo supports PathPlanner // 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; severity = SYSTEMALARMS_ALARM_ERROR;
} }
}
break; break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) { if (coptercontrol) {

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

@ -135,16 +135,20 @@ static void pathPlannerTask()
// check flight plan validity early to raise alarm // check flight plan validity early to raise alarm
// even if not in guided mode // even if not in guided mode
uint8_t validFlightPlan = checkFlightPlan(); uint8_t validFlightPlan = checkFlightPlan();
if (!validFlightPlan) {
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
}
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false; 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; return;
} }
@ -156,7 +160,9 @@ static void pathPlannerTask()
if (!validFlightPlan) { if (!validFlightPlan) {
// At this point the craft is in PathPlanner mode, so pilot has no manual control capability. // 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 // 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; pathplanner_active = false;
if (!failsafeRTHset) { if (!failsafeRTHset) {
@ -178,8 +184,12 @@ static void pathPlannerTask()
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
return;
} }
failsafeRTHset = 0; failsafeRTHset = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
WaypointActiveGet(&waypointActive); WaypointActiveGet(&waypointActive);