diff --git a/src/widgets/map/acitemmanager.cpp b/src/widgets/map/acitemmanager.cpp index a79bfbd4..74f4d5b7 100644 --- a/src/widgets/map/acitemmanager.cpp +++ 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; + } } diff --git a/src/widgets/map/acitemmanager.h b/src/widgets/map/acitemmanager.h index 4a65228b..b15084b8 100644 --- a/src/widgets/map/acitemmanager.h +++ b/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; }; diff --git a/src/widgets/map/graphics_objects/graphics_object.h b/src/widgets/map/graphics_objects/graphics_object.h index 8df6d172..47c90499 100644 --- a/src/widgets/map/graphics_objects/graphics_object.h +++ b/src/widgets/map/graphics_objects/graphics_object.h @@ -18,6 +18,7 @@ class GraphicsObject : public QObject CARROT, GCS, DCSHOT, + CRASH, }; enum Animation { diff --git a/src/widgets/map/graphics_objects/graphics_point.cpp b/src/widgets/map/graphics_objects/graphics_point.cpp index f7bfb2ac..29173612 100644 --- a/src/widgets/map/graphics_objects/graphics_point.cpp +++ b/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; } } diff --git a/src/widgets/map/graphics_objects/graphics_text.cpp b/src/widgets/map/graphics_objects/graphics_text.cpp index 08618a71..63e4ce28 100644 --- a/src/widgets/map/graphics_objects/graphics_text.cpp +++ b/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(); diff --git a/src/widgets/map/mapwidget.cpp b/src/widgets/map/mapwidget.cpp index 45f1682f..796d4833 100644 --- a/src/widgets/map/mapwidget.cpp +++ b/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* 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 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 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); + } + } +} diff --git a/src/widgets/map/mapwidget.h b/src/widgets/map/mapwidget.h index 7de33676..6fcfee29 100644 --- a/src/widgets/map/mapwidget.h +++ b/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