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

- 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)

flight/modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -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
  *

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
+    FlightPlanGet(&flightPlan);
-    if (num >= UAVObjGetNumInstances(WaypointHandle())) {
+
+    // 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;
     }
 

ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp

@@ -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;
+    // we will start 3 update all
-    completionSuccessCount = 0;
+    completionCountdown = 3;
+    successCountdown = completionCountdown;
 
     flightPlan->updated();
     waypoint->updatedAll();
@@ -63,18 +70,16 @@ void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success)
 {
     obj->disconnect(this);
 
-    completionCount++;
+    completionCountdown--;
-    if (success) {
+    successCountdown -= success ? 1 : 0;
-        completionSuccessCount++;
-    }
 
-    if (completionCount == 3) {
+    if (completionCountdown == 0) {
-        qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (completionSuccessCount == 3);
+        qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (successCountdown == 0);
-        if (completionSuccessCount == 3) {
+        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;
+    // we will start 3 update requests
-    completionSuccessCount = 0;
+    completionCountdown = 3;
+    successCountdown = completionCountdown;
 
     flightPlan->requestUpdate();
     waypoint->requestUpdateAll();
@@ -102,19 +108,18 @@ void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success)
 {
     obj->disconnect(this);
 
-    completionCount++;
+    completionCountdown--;
-    if (success) {
+    successCountdown -= success ? 1 : 0;
-        completionSuccessCount++;
-    }
 
-    if (completionCount == 3) {
+    if (completionCountdown == 0) {
-        qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (completionSuccessCount == 3);
+        qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (successCountdown == 0);
-        if (completionSuccessCount == 3) {
+        if (successCountdown == 0) {
-            objectsToModel();
+            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 = computeFlightPlanCrc(waypointCount, actionCount);
-    flightPlanData.Crc = 0;
 
     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
+    // consistency check
-    // objMngr->getNumInstances(Waypoint::OBJID);
+    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;
+}
+

ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h

@@ -49,11 +49,11 @@ private:
     UAVObjectManager *objMngr;
     flightDataModel *myModel;
 
-    uint completionCount;
+    uint completionCountdown;
-    uint completionSuccessCount;
+    uint successCountdown;
 
-    void modelToObjects();
+    bool modelToObjects();
-    void objectsToModel();
+    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