rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1122 OP-1158 added flight plan CRC consistency checks (in GCS and fixedwingpathfollower module)

Browse Source 
- need to do the same in vtolpathfollower module)
- addressed an issue that could cause unwanted waypoints to be used (path follower was looping over all instanciated wp)
This commit is contained in:
Philippe Renon 2014-01-12 19:42:12 +01:00
parent ff8a001a51
commit a64720a9f1
4 changed files with 133 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -45,7 +45,13 @@
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <uavobjectmanager.h>
#include "hwsettings.h"
#include "flightplan.h"
#include "waypoint.h"
#include "pathaction.h"
#include "attitudestate.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionstate.h"
@ -62,7 +68,6 @@
#include "velocitydesired.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include <pios_struct_helper.h>
#include "paths.h"
#include "CoordinateConversions.h"
@ -80,6 +85,7 @@ static PathStatusData pathStatus;
static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
// Private functions
static bool checkFlightPlan();
static void pathfollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void updatePathVelocity();
@ -163,6 +169,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
// 3. FlightPlan must be valid
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
@ -172,11 +179,15 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
vTaskDelay(1000);
continue;
}
if (!checkFlightPlan()) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
@ -242,6 +253,48 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
}
}
static bool 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;
}
/**
* Compute desired velocity from the current position and path
*

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

@ -268,10 +268,13 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num)
{
// path plans wrap around
FlightPlanData flightPlan;
// TODO change to FlightPlan.WaypointCount
if (num >= UAVObjGetNumInstances(WaypointHandle())) {
FlightPlanGet(&flightPlan);
// here it is assumed that the flight plan has been validated (waypoint count is consistent)
if (num >= flightPlan.WaypointCount) {
// path plans wrap around
num = 0;
}

97
ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp
View File

@ -36,6 +36,9 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec
objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr != NULL);
completionCountdown = 0;
successCountdown = 0;
}
void ModelUavoProxy::sendFlightPlan()
@ -43,16 +46,20 @@ void ModelUavoProxy::sendFlightPlan()
modelToObjects();
FlightPlan *flightPlan = FlightPlan::GetInstance(objMngr, 0);
connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool)));
connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool)));
connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
PathAction *action = PathAction::GetInstance(objMngr, 0);
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool)));
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)),
this, SLOT(flightPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
completionCount = 0;
completionSuccessCount = 0;
// we will start 3 update all
completionCountdown = 3;
successCountdown = completionCountdown;
flightPlan->updated();
waypoint->updatedAll();
@ -63,18 +70,16 @@ void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success)
{
obj->disconnect(this);
completionCount++;
if (success) {
completionSuccessCount++;
}
completionCountdown--;
successCountdown -= success ? 1 : 0;
if (completionCount == 3) {
qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (completionSuccessCount == 3);
if (completionSuccessCount == 3) {
if (completionCountdown == 0) {
qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (successCountdown == 0);
if (successCountdown == 0) {
QMessageBox::information(NULL, tr("Flight Plan Upload Successful"), tr("Flight plan upload was successful."));
}
else {
QMessageBox::critical(NULL, tr("Flight Plan Upload Failed !"), tr("Failed to upload the flight plan !"));
QMessageBox::critical(NULL, tr("Flight Plan Upload Failed"), tr("Failed to upload the flight plan !"));
}
}
}
@ -90,8 +95,9 @@ void ModelUavoProxy::receiveFlightPlan()
PathAction *action = PathAction::GetInstance(objMngr, 0);
connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool)));
completionCount = 0;
completionSuccessCount = 0;
// we will start 3 update requests
completionCountdown = 3;
successCountdown = completionCountdown;
flightPlan->requestUpdate();
waypoint->requestUpdateAll();
@ -102,19 +108,18 @@ void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success)
{
obj->disconnect(this);
completionCount++;
if (success) {
completionSuccessCount++;
}
completionCountdown--;
successCountdown -= success ? 1 : 0;
if (completionCount == 3) {
qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (completionSuccessCount == 3);
if (completionSuccessCount == 3) {
objectsToModel();
if (completionCountdown == 0) {
qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (successCountdown == 0);
if (successCountdown == 0) {
if (objectsToModel()) {
QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful."));
}
}
else {
QMessageBox::critical(NULL, tr("Flight Plan Download Failed !"), tr("Failed to download the flight plan !"));
QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Failed to download the flight plan !"));
}
}
}
@ -129,7 +134,7 @@ void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success)
// (compression consists in keeping only one instance of similar path actions)
//
// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances
void ModelUavoProxy::modelToObjects()
bool ModelUavoProxy::modelToObjects()
{
qDebug() << "ModelUAVProxy::modelToObjects";
@ -186,13 +191,13 @@ void ModelUavoProxy::modelToObjects()
// update UAVObject
waypoint->setData(waypointData);
}
// Put "safe" values in unused waypoint objects
// Put "safe" values in unused waypoint and path action objects
if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) {
for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) {
// TODO
}
}
// Put "safe" values in unused path action objects
if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) {
for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) {
// TODO
@ -205,10 +210,11 @@ void ModelUavoProxy::modelToObjects()
flightPlanData.WaypointCount = waypointCount;
flightPlanData.PathActionCount = actionCount;
// TODO
flightPlanData.Crc = 0;
flightPlanData.Crc = computeFlightPlanCrc(waypointCount, actionCount);
flightPlan->setData(flightPlanData);
return true;
}
Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) {
@ -287,7 +293,7 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD
return NULL;
}
void ModelUavoProxy::objectsToModel()
bool ModelUavoProxy::objectsToModel()
{
// build model from uav objects
// the list of objects can end with "garbage" instances due to previous flightpath
@ -297,9 +303,21 @@ void ModelUavoProxy::objectsToModel()
FlightPlan::DataFields flightPlanData = flightPlan->getData();
int waypointCount = flightPlanData.WaypointCount;
int actionCount = flightPlanData.PathActionCount;
// TODO consistency checks
// objMngr->getNumInstances(Waypoint::OBJID);
// consistency check
if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Flight plan way point count error !"));
return false;
}
if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) {
QMessageBox::critical(NULL, tr("Flight Plan Download Failed"), tr("Flight plan path action count error !"));
return false;
}
if (flightPlanData.Crc != computeFlightPlanCrc(waypointCount, actionCount)) {
QMessageBox::critical(NULL, tr("Flight Plan Upload Failed"), tr("Flight plan CRC error !"));
return false;
}
int rowCount = myModel->rowCount();
if (waypointCount < rowCount) {
@ -328,6 +346,7 @@ void ModelUavoProxy::objectsToModel()
PathAction::DataFields actionData = action->getData();
pathActionToModel(i, actionData);
}
return true;
}
void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) {
@ -436,3 +455,17 @@ void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data) {
index = myModel->index(i, flightDataModel::MODE_PARAMS3);
myModel->setData(index, data.ModeParameters[3]);
}
quint8 ModelUavoProxy::computeFlightPlanCrc(int waypointCount, int actionCount) {
quint8 crc = 0;
for (int i = 0; i < waypointCount; ++i) {
Waypoint* waypoint = Waypoint::GetInstance(objMngr, i);
crc = waypoint->updateCRC(crc);
}
for (int i = 0; i < actionCount; ++i) {
PathAction* action = PathAction::GetInstance(objMngr, i);
crc = action->updateCRC(crc);
}
return crc;
}

11
ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h
View File

@ -49,11 +49,11 @@ private:
UAVObjectManager *objMngr;
flightDataModel *myModel;
uint completionCount;
uint completionSuccessCount;
uint completionCountdown;
uint successCountdown;
void modelToObjects();
void objectsToModel();
bool modelToObjects();
bool objectsToModel();
Waypoint *createWaypoint(int index, Waypoint *newWaypoint);
PathAction *createPathAction(int index, PathAction *newAction);
@ -66,10 +66,11 @@ private:
void waypointToModel(int i, Waypoint::DataFields &data);
void pathActionToModel(int i, PathAction::DataFields &data);
quint8 computeFlightPlanCrc(int waypointCount, int actionCount);
private slots:
void flightPlanElementSent(UAVObject *, bool success);
void flightPlanElementReceived(UAVObject *, bool success);
};
#endif // MODELUAVOPROXY_H