1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00

OP-1122 add flightplan safety check to pathplanner - failsafe rth when flightmode is set to pathplan but no valid plan is uploaded

This commit is contained in:
Corvus Corax 2014-01-12 21:04:31 +01:00
parent b8118f51e7
commit cb9e76b006
2 changed files with 87 additions and 3 deletions

View File

@ -41,7 +41,7 @@
#include "velocitystate.h"
#include "waypoint.h"
#include "waypointactive.h"
#include "taskinfo.h"
#include "manualcontrolsettings.h"
#include <pios_struct_helper.h>
#include "paths.h"
@ -60,6 +60,7 @@ static void statusUpdated(UAVObjEvent *ev);
static void updatePathDesired();
static void setWaypoint(uint16_t num);
static uint8_t checkFlightPlan();
static uint8_t pathConditionCheck();
static uint8_t conditionNone();
static uint8_t conditionTimeOut();
@ -131,6 +132,15 @@ static void pathPlannerTask()
bool endCondition = false;
// 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) {
@ -139,6 +149,38 @@ static void pathPlannerTask()
return;
}
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
static uint8_t failsafeRTHset = 0;
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.
pathplanner_active = false;
if (!failsafeRTHset) {
failsafeRTHset = 1;
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
PositionStateData positionState;
PositionStateGet(&positionState);
ManualControlSettingsData settings;
ManualControlSettingsGet(&settings);
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
failsafeRTHset = 0;
WaypointActiveGet(&waypointActive);
if (pathplanner_active == false) {
@ -155,8 +197,6 @@ static void pathPlannerTask()
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
@ -207,6 +247,49 @@ static void pathPlannerTask()
}
}
// safety check for path plan integrity
static uint8_t checkFlightPlan()
{
uint16_t i;
uint16_t waypointCount;
uint16_t actionCount;
uint8_t flightCrc;
FlightPlanData flightPlan;
FlightPlanGet(&flightPlan);
waypointCount = flightPlan.WaypointCount;
actionCount = flightPlan.PathActionCount;
// check count consistency
if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
PIOS_DEBUGLOG_Printf("FlighPlan : waypoint count error!");
return false;
}
if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
PIOS_DEBUGLOG_Printf("FlighPlan : path action count error!");
return false;
}
// check CRC
flightCrc = 0;
for (i = 0; i < waypointCount; i++) {
flightCrc = UAVObjUpdateCRC(WaypointHandle(), i, flightCrc);
}
for (i = 0; i < actionCount; i++) {
flightCrc = UAVObjUpdateCRC(PathActionHandle(), i, flightCrc);
}
if (flightCrc != flightPlan.Crc) {
PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", flightCrc, flightPlan.Crc);
return false;
}
// everything ok (hopefully...)
PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", flightCrc, flightPlan.Crc);
return true;
}
// callback function when status changed, issue execution of state machine
void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
{

View File

@ -16,6 +16,7 @@
<elementname>Sensors</elementname>
<elementname>Stabilization</elementname>
<elementname>Guidance</elementname>
<elementname>PathPlan</elementname>
<elementname>Battery</elementname>
<elementname>FlightTime</elementname>
<elementname>I2C</elementname>