Merge pull request #24 from EwoudSmeur/crash_prediction

predict crash location on GCS

Author: Fabien-B

src/widgets/map/acitemmanager.cpp

@@ -1,9 +1,9 @@
 #include "acitemmanager.h"
 
-ACItemManager::ACItemManager(QString ac_id, WaypointItem* target, AircraftItem* aircraft_item, ArrowItem* arrow, QObject* parent):
+ACItemManager::ACItemManager(QString ac_id, WaypointItem* target, AircraftItem* aircraft_item, ArrowItem* arrow, WaypointItem* crash_item, QObject* parent):
     QObject(parent),
     ac_id(ac_id), target(target), aircraft_item(aircraft_item),
-    current_nav_shape(nullptr), max_dist_circle(nullptr), arrow_item(arrow), gvf_trajectory(nullptr)
+    current_nav_shape(nullptr), max_dist_circle(nullptr), arrow_item(arrow), crash_item(crash_item), gvf_trajectory(nullptr)
 {
 
 }
@@ -72,4 +72,9 @@ void ACItemManager::removeItems(MapWidget* map) {
         map->removeItem(max_dist_circle);
         max_dist_circle = nullptr;
     }
+
+    if(crash_item) {
+        map->removeItem(crash_item);
+        crash_item = nullptr;
+    }
 }

src/widgets/map/acitemmanager.h

@@ -14,7 +14,7 @@ class ACItemManager: public QObject
 {
     Q_OBJECT
 public:
-    ACItemManager(QString ac_id, WaypointItem* target, AircraftItem* aircraft_item, ArrowItem* arrow_item, QObject* parent=nullptr);
+    ACItemManager(QString ac_id, WaypointItem* target, AircraftItem* aircraft_item, ArrowItem* arrow_item, WaypointItem* crash_item, QObject* parent=nullptr);
 
     void addWaypointItem(WaypointItem*);
     void addPathItem(PathItem*);
@@ -27,6 +27,7 @@ class ACItemManager: public QObject
     MapItem* getCurrentNavShape() {return current_nav_shape;}
     AircraftItem* getAircraftItem() {return aircraft_item;}
     ArrowItem* getArrowItem() {return arrow_item;}
+    WaypointItem* getCrashItem() {return crash_item;}
     void removeItems(MapWidget* map);
 
 private:
@@ -38,6 +39,7 @@ class ACItemManager: public QObject
     MapItem* current_nav_shape;
     CircleItem* max_dist_circle;
     ArrowItem* arrow_item;
+    WaypointItem* crash_item;
 
     GVF_trajectory* gvf_trajectory;
 };

src/widgets/map/graphics_objects/graphics_object.h

@@ -18,6 +18,7 @@ class GraphicsObject : public QObject
         CARROT,
         GCS,
         DCSHOT,
+        CRASH,
     };
 
     enum Animation {

src/widgets/map/graphics_objects/graphics_point.cpp

@@ -97,6 +97,13 @@ void GraphicsPoint::paint(QPainter *painter, const QStyleOptionGraphicsItem *opt
         painter->drawEllipse(QPoint(0, 0), halfSize/2, halfSize/2);
         }
         break;
+    case CRASH: {
+        // Draw a red cross with thickness 3
+        painter->setPen(QPen(QColor(255, 0, 0), 3));
+        painter->drawLine(QPoint(-halfSize/2, -halfSize/2), QPoint(halfSize/2, halfSize/2));
+        painter->drawLine(QPoint(-halfSize/2, halfSize/2), QPoint(halfSize/2, -halfSize/2));
+        }
+        break;
     }
 
 }

src/widgets/map/graphics_objects/graphics_text.cpp

@@ -31,6 +31,9 @@ void GraphicsText::changeFocus() {
     case DCSHOT:
         setVisible(false);
         break;
+    case CRASH:
+        setVisible(false);
+        break;
     }
 
     update();

src/widgets/map/mapwidget.cpp

@@ -29,6 +29,7 @@
 #include "intruder_item.h"
 #include "arrow_item.h"
 #include "pprzmain.h"
+#include "coordinatestransform.h"
 
 #include "quiver_item.h"
 #include "gvf_traj_line.h"
@@ -113,6 +114,13 @@ MapWidget::MapWidget(QWidget *parent) : Map2D(parent),
     connect(show_hidden_wp_action, &QAction::toggled, [=](bool show) {
         setProperty("show_hidden_waypoints", show);
     });
+
+    show_crash_prediction_action = mapMenu->addAction("Show crash prediction");
+    show_crash_prediction_action->setCheckable(true);
+    connect(show_crash_prediction_action, &QAction::toggled, [=](bool show) {
+        setProperty("show_crash_prediction", show);
+    });
+
     auto clear_shapes = mapMenu->addAction("Clear Shapes");
     connect(clear_shapes, &QAction::triggered, this, [=](){
         clearShapes();
@@ -134,11 +142,11 @@ MapWidget::MapWidget(QWidget *parent) : Map2D(parent),
         [=](QString sender, QVector<int>* gvfViewer_config) {
             gvf_trajectories_config.remove(sender);
             gvf_trajectories_config[sender] = gvfViewer_config;
-        });  
+        });
 
     connect(  DispatcherUi::get(), &DispatcherUi::new_ac_config, this, &MapWidget::handleNewAC);
     connect(  DispatcherUi::get(), &DispatcherUi::ac_deleted, this, &MapWidget::removeAC);
-    connect(  DispatcherUi::get(), &DispatcherUi::ac_selected, this, &MapWidget::changeCurrentAC);  
+    connect(  DispatcherUi::get(), &DispatcherUi::ac_selected, this, &MapWidget::changeCurrentAC);
     connect(  DispatcherUi::get(), &DispatcherUi::centerMap, this, &MapWidget::centerLatLon);
     connect(PprzDispatcher::get(), &PprzDispatcher::flight_param, this, &MapWidget::updateAircraftItem);
     connect(PprzDispatcher::get(), &PprzDispatcher::nav_status, this, &MapWidget::updateTarget);
@@ -188,6 +196,11 @@ MapWidget::MapWidget(QWidget *parent) : Map2D(parent),
             onGVF(sender, msg);
         });
 
+    PprzDispatcher::get()->bind("ROTORCRAFT_FP", this,
+        [=](QString sender, pprzlink::Message msg) {
+            onROTORCRAFT_FP(sender, msg);
+        });
+
     setAcceptDrops(true);
 
     // Add menu to app menu bar.
@@ -747,6 +760,16 @@ void MapWidget::handleNewAC(QString ac_id) {
         double z_carrot = settings.value("map/z_values/carrot").toDouble();
         target->setZValues(z_carrot, z_carrot);
 
+        // create crash item at dummy position
+        auto crash_item = new WaypointItem(Point2DLatLon(0, 0), ac_id, 16);
+        crash_item->setStyle(GraphicsObject::Style::CRASH);
+        if(!show_crash_prediction_action->isChecked()) {
+            crash_item->setVisible(false);
+        } else {
+            crash_item->setVisible(true);
+        }
+        addItem(crash_item);
+
         ArrowItem* arrow = new ArrowItem(ac_id, 15, this);
         addItem(arrow);
         arrow->setProperty("size", _ac_arrow_size);
@@ -757,7 +780,7 @@ void MapWidget::handleNewAC(QString ac_id) {
         });
 
         //create the ACItemManager for this aircraft
-        item_manager = new ACItemManager(ac_id, target, aircraft_item, arrow, this);
+        item_manager = new ACItemManager(ac_id, target, aircraft_item, arrow, crash_item, this);
 
         auto clear_track = new QAction(ac->name(), ac);
         connect(clear_track, &QAction::triggered, aircraft_item, [=](){
@@ -767,7 +790,7 @@ void MapWidget::handleNewAC(QString ac_id) {
 
     } else {
         //create the ACItemManager for this fake aircraft (flightplan only)
-        item_manager = new ACItemManager(ac_id, nullptr, nullptr, nullptr, this);
+        item_manager = new ACItemManager(ac_id, nullptr, nullptr, nullptr, nullptr, this);
     }
 
     ac_items_managers[ac_id] = item_manager;
@@ -1196,6 +1219,47 @@ void MapWidget::onDcShot(QString sender, pprzlink::Message msg) {
     dc_shots.append(dsw);
 }
 
+void MapWidget::onROTORCRAFT_FP(QString sender, pprzlink::Message msg) {
+
+    int32_t east, north, up, vnorth, veast, vup;
+    msg.getField("east", east);
+    msg.getField("north", north);
+    msg.getField("up", up);
+    msg.getField("vnorth", vnorth);
+    msg.getField("veast", veast);
+    msg.getField("vup", vup);
+
+  if(AircraftManager::get()->aircraftExists(sender)) {
+    ac_items_managers[sender]->getCrashItem();
+
+    float g = -9.81f;
+
+    double vx = veast*0.00000190734;
+    double vy = vnorth*0.00000190734;
+    double vz = vup*0.00000190734;
+
+    double x = east*0.0039063;
+    double y = north*0.0039063;
+    double z = up*0.0039063;
+
+    double h = fabs(z); // Should be height above ground, make sure to initialize local frame on ground
+
+    // With h always larger than 0, the sqrt can never give nan
+    float time_fall = (-vz - sqrtf(vz*vz -2.f*h*g))/g;
+
+    double x_pos = x + time_fall*vx;
+    double y_pos = y + time_fall*vy;
+
+    auto ac = AircraftManager::get()->getAircraft(sender);
+    auto orig = ac->getFlightPlan()->getOrigin();
+    Point2DLatLon pos(orig);
+
+    Point2DLatLon markerpos = CoordinatesTransform::get()->ltp_to_wgs84(pos,x_pos,y_pos);
+
+    ac_items_managers[sender]->getCrashItem()->setPosition(markerpos);
+  }
+}
+
 void MapWidget::onGCSPos(pprzlink::Message msg) {
     if(gcsItem) {
         removeItem(gcsItem);
@@ -1228,7 +1292,7 @@ void MapWidget::setAcArrowSize(int s) {
 }
 
 void MapWidget::onGVF(QString sender, pprzlink::Message msg) {
-    
+
     if(!gvf_loaded) {
         return;
     }
@@ -1245,7 +1309,7 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) {
 
         gvf_trajectories[sender]->purge_trajectory();
         delete gvf_trajectories[sender];
-        
+
         gvf_trajectories.remove(sender);
         ac_items_managers[sender]->setCurrentGVF(nullptr);
     }
@@ -1256,7 +1320,7 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) {
     QList<float> param = {0.0};
     int8_t direction;
     GVF_trajectory* gvf_traj;
-    
+
     // GVF message parser
     if(msg.getDefinition().getName() == "GVF") {
         float error, ke;
@@ -1268,7 +1332,7 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) {
         msg.getField("p", param);
 
         switch(traj)
-        {   
+        {
             case 0: {// Straight line
                 gvf_traj = new GVF_traj_line(sender, param, direction, ke, gvf_trajectories_config[sender]);
                 break;
@@ -1290,14 +1354,14 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) {
     // GVF_PARAMETRIC
     else if (msg.getDefinition().getName() == "GVF_PARAMETRIC") {
         QList<float> phi = {0.0}; // Error signals
-        
+
         msg.getField("traj", traj);
         msg.getField("w", wb);
         msg.getField("p", param);
         msg.getField("phi", phi);
 
         switch(traj)
-        {   
+        {
             case 0: {// Trefoil 2D
                 gvf_traj = new GVF_traj_trefoil(sender, param, phi, gvf_trajectories_config[sender]);
                 break;
@@ -1321,7 +1385,7 @@ void MapWidget::onGVF(QString sender, pprzlink::Message msg) {
     } else {
         return;
     }
-    
+
     addItem(gvf_traj->getTraj());
     addItem(gvf_traj->getVField());
     ac_items_managers[sender]->setCurrentGVF(gvf_traj);
@@ -1344,3 +1408,16 @@ void MapWidget::showHiddenWaypoints(bool state) {
         }
     }
 }
+
+void MapWidget::showCrashPrediction(bool state) {
+    show_crash_prediction_action->blockSignals(true);
+    show_crash_prediction_action->setChecked(state);
+    show_crash_prediction_action->blockSignals(false);
+    for(auto &itemManager: ac_items_managers) {
+        if(state) {
+            itemManager->getCrashItem()->setVisible(true);
+        } else {
+            itemManager->getCrashItem()->setVisible(false);
+        }
+    }
+}

src/widgets/map/mapwidget.h

@@ -42,6 +42,7 @@ class MapWidget : public Map2D, public Configurable
     Q_OBJECT
     Q_PROPERTY(int ac_arrow_size MEMBER _ac_arrow_size WRITE setAcArrowSize)
     Q_PROPERTY(bool show_hidden_waypoints WRITE showHiddenWaypoints)
+    Q_PROPERTY(bool show_crash_prediction WRITE showCrashPrediction)
 public:
     explicit MapWidget(QWidget *parent = nullptr);
 
@@ -61,6 +62,7 @@ class MapWidget : public Map2D, public Configurable
     void rotateMap(double rot);
     void setAcArrowSize(int s);
     void showHiddenWaypoints(bool state);
+    void showCrashPrediction(bool state);
 
 signals:
     void mouseMoved(QPointF scenePos);
@@ -91,6 +93,7 @@ private slots:
     void clearDcShots();
     void onIntruder(QString sender, pprzlink::Message msg);
     void onDcShot(QString sender, pprzlink::Message msg);
+    void onROTORCRAFT_FP(QString sender, pprzlink::Message msg);
     void onGCSPos(pprzlink::Message msg);
     void onGVF(QString sender, pprzlink::Message msg);
 
@@ -154,6 +157,7 @@ private slots:
     QMenu* mapMenu;
     QMenu* menu_clear_track;
     QAction* show_hidden_wp_action;
+    QAction* show_crash_prediction_action;
 };
 
 #endif // MAPWIDGET_H