diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 5858bf5e0..3226d80bd 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -45,7 +45,13 @@ #include +#include +#include + #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 #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 * diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index f62e0e2af..eb43352c2 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -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; } diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index d6e9945f1..dfa6b4818 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -36,6 +36,9 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec objMngr = pm->getObject(); 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(); - QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); + 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; +} + diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 23e81557b..b1946c1dc 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -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