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:
parent
b8118f51e7
commit
cb9e76b006
@ -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)
|
||||
{
|
||||
|
@ -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>
|
||||
|
Loading…
x
Reference in New Issue
Block a user