From 0ec7ac70bcac4c3dd27dab2f151684aa24081547 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Tue, 13 Feb 2024 18:03:17 +0100 Subject: [PATCH 1/4] predict crash location on GCS --- .../map/graphics_objects/graphics_object.h | 1 + .../map/graphics_objects/graphics_point.cpp | 7 ++ .../map/graphics_objects/graphics_text.cpp | 3 + src/widgets/map/mapwidget.cpp | 78 ++++++++++++++++--- src/widgets/map/mapwidget.h | 2 + 5 files changed, 81 insertions(+), 10 deletions(-) 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..f6f471f4 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" @@ -41,7 +42,7 @@ MapWidget::MapWidget(QWidget *parent) : Map2D(parent), - interaction_state(PMIS_OTHER), drawState(false), fp_edit_sm(nullptr), gcsItem(nullptr), + interaction_state(PMIS_OTHER), drawState(false), fp_edit_sm(nullptr), gcsItem(nullptr),crashItem(nullptr), pan_state(PAN_IDLE), pan_mouse_mask(Qt::MiddleButton | Qt::LeftButton), _ac_arrow_size(30) { @@ -134,11 +135,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 +189,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. @@ -1196,6 +1202,58 @@ void MapWidget::onDcShot(QString sender, pprzlink::Message msg) { dc_shots.append(dsw); } +void MapWidget::onROTORCRAFT_FP(QString sender, pprzlink::Message msg) { + (void)sender; + + if(crashItem) { + removeItem(crashItem); + } + + 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(current_ac)) { + + 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(current_ac); + auto orig = ac->getFlightPlan()->getOrigin(); + Point2DLatLon pos(orig); + + Point2DLatLon markerpos = CoordinatesTransform::get()->ltp_to_wgs84(pos,x_pos,y_pos); + + auto p = PprzPalette(QColor(Qt::yellow), QBrush(Qt::yellow)); + + auto crashpos = new WaypointItem(markerpos,"__NO_AC__", p , 15, this); + crashpos->setStyle(GraphicsObject::Style::CRASH); + crashpos->setEditable(false); + addItem(crashpos); + + crashItem = crashpos; + } +} + void MapWidget::onGCSPos(pprzlink::Message msg) { if(gcsItem) { removeItem(gcsItem); @@ -1228,7 +1286,7 @@ void MapWidget::setAcArrowSize(int s) { } void MapWidget::onGVF(QString sender, pprzlink::Message msg) { - + if(!gvf_loaded) { return; } @@ -1245,7 +1303,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 +1314,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 +1326,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 +1348,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 +1379,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); diff --git a/src/widgets/map/mapwidget.h b/src/widgets/map/mapwidget.h index 7de33676..f52c8121 100644 --- a/src/widgets/map/mapwidget.h +++ b/src/widgets/map/mapwidget.h @@ -91,6 +91,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); @@ -125,6 +126,7 @@ private slots: QList dc_shots; QTimer* timer_intruders; MapItem* gcsItem; + MapItem* crashItem; QHBoxLayout* horizontalLayout; From ac11b409bf32d228aa01b0e8b9a1ad4f59eea432 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Wed, 21 Feb 2024 11:57:34 +0100 Subject: [PATCH 2/4] Crashitem as part of AC --- src/widgets/map/acitemmanager.cpp | 9 +++++++-- src/widgets/map/acitemmanager.h | 4 +++- src/widgets/map/mapwidget.cpp | 32 +++++++++++++------------------ src/widgets/map/mapwidget.h | 1 - 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/widgets/map/acitemmanager.cpp b/src/widgets/map/acitemmanager.cpp index a79bfbd4..8e35fea2 100644 --- a/src/widgets/map/acitemmanager.cpp +++ b/src/widgets/map/acitemmanager.cpp @@ -1,8 +1,8 @@ #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), + ac_id(ac_id), target(target), aircraft_item(aircraft_item), crash_item(crash_item), current_nav_shape(nullptr), max_dist_circle(nullptr), arrow_item(arrow), 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/mapwidget.cpp b/src/widgets/map/mapwidget.cpp index f6f471f4..1a59391f 100644 --- a/src/widgets/map/mapwidget.cpp +++ b/src/widgets/map/mapwidget.cpp @@ -42,7 +42,7 @@ MapWidget::MapWidget(QWidget *parent) : Map2D(parent), - interaction_state(PMIS_OTHER), drawState(false), fp_edit_sm(nullptr), gcsItem(nullptr),crashItem(nullptr), + interaction_state(PMIS_OTHER), drawState(false), fp_edit_sm(nullptr), gcsItem(nullptr), pan_state(PAN_IDLE), pan_mouse_mask(Qt::MiddleButton | Qt::LeftButton), _ac_arrow_size(30) { @@ -753,6 +753,11 @@ 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); + addItem(crash_item); + ArrowItem* arrow = new ArrowItem(ac_id, 15, this); addItem(arrow); arrow->setProperty("size", _ac_arrow_size); @@ -763,7 +768,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, [=](){ @@ -773,7 +778,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; @@ -1203,11 +1208,6 @@ void MapWidget::onDcShot(QString sender, pprzlink::Message msg) { } void MapWidget::onROTORCRAFT_FP(QString sender, pprzlink::Message msg) { - (void)sender; - - if(crashItem) { - removeItem(crashItem); - } int32_t east, north, up, vnorth, veast, vup; msg.getField("east", east); @@ -1217,7 +1217,8 @@ void MapWidget::onROTORCRAFT_FP(QString sender, pprzlink::Message msg) { msg.getField("veast", veast); msg.getField("vup", vup); - if(AircraftManager::get()->aircraftExists(current_ac)) { + if(AircraftManager::get()->aircraftExists(sender)) { + ac_items_managers[sender]->getCrashItem(); float g = -9.81f; @@ -1237,21 +1238,14 @@ void MapWidget::onROTORCRAFT_FP(QString sender, pprzlink::Message msg) { double x_pos = x + time_fall*vx; double y_pos = y + time_fall*vy; - auto ac = AircraftManager::get()->getAircraft(current_ac); + 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); - auto p = PprzPalette(QColor(Qt::yellow), QBrush(Qt::yellow)); - - auto crashpos = new WaypointItem(markerpos,"__NO_AC__", p , 15, this); - crashpos->setStyle(GraphicsObject::Style::CRASH); - crashpos->setEditable(false); - addItem(crashpos); - - crashItem = crashpos; - } + ac_items_managers[sender]->getCrashItem()->setPosition(markerpos); + } } void MapWidget::onGCSPos(pprzlink::Message msg) { diff --git a/src/widgets/map/mapwidget.h b/src/widgets/map/mapwidget.h index f52c8121..6a4851e9 100644 --- a/src/widgets/map/mapwidget.h +++ b/src/widgets/map/mapwidget.h @@ -126,7 +126,6 @@ private slots: QList dc_shots; QTimer* timer_intruders; MapItem* gcsItem; - MapItem* crashItem; QHBoxLayout* horizontalLayout; From 6ec40fc91be8ce30f7318710dccbe591530ee6d8 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Wed, 21 Feb 2024 15:42:37 +0100 Subject: [PATCH 3/4] checkbox to turn on crash prediction --- src/widgets/map/acitemmanager.cpp | 4 ++-- src/widgets/map/mapwidget.cpp | 25 +++++++++++++++++++++++++ src/widgets/map/mapwidget.h | 3 +++ 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/widgets/map/acitemmanager.cpp b/src/widgets/map/acitemmanager.cpp index 8e35fea2..74f4d5b7 100644 --- a/src/widgets/map/acitemmanager.cpp +++ b/src/widgets/map/acitemmanager.cpp @@ -2,8 +2,8 @@ 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), crash_item(crash_item), - current_nav_shape(nullptr), max_dist_circle(nullptr), arrow_item(arrow), gvf_trajectory(nullptr) + ac_id(ac_id), target(target), aircraft_item(aircraft_item), + current_nav_shape(nullptr), max_dist_circle(nullptr), arrow_item(arrow), crash_item(crash_item), gvf_trajectory(nullptr) { } diff --git a/src/widgets/map/mapwidget.cpp b/src/widgets/map/mapwidget.cpp index 1a59391f..0c11392c 100644 --- a/src/widgets/map/mapwidget.cpp +++ b/src/widgets/map/mapwidget.cpp @@ -114,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(); @@ -756,6 +763,11 @@ void MapWidget::handleNewAC(QString ac_id) { // 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->setSize(0); + } else { + crash_item->setSize(5); + } addItem(crash_item); ArrowItem* arrow = new ArrowItem(ac_id, 15, this); @@ -1396,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()->setSize(5*2); + } else { + itemManager->getCrashItem()->setSize(0); + } + } +} diff --git a/src/widgets/map/mapwidget.h b/src/widgets/map/mapwidget.h index 6a4851e9..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); @@ -155,6 +157,7 @@ private slots: QMenu* mapMenu; QMenu* menu_clear_track; QAction* show_hidden_wp_action; + QAction* show_crash_prediction_action; }; #endif // MAPWIDGET_H From 14eebfda16af85aaae32a378b3dda060c4b279a3 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Wed, 21 Feb 2024 18:16:30 +0100 Subject: [PATCH 4/4] hide crash prediction --- src/widgets/map/mapwidget.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/widgets/map/mapwidget.cpp b/src/widgets/map/mapwidget.cpp index 0c11392c..796d4833 100644 --- a/src/widgets/map/mapwidget.cpp +++ b/src/widgets/map/mapwidget.cpp @@ -764,9 +764,9 @@ void MapWidget::handleNewAC(QString ac_id) { 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->setSize(0); + crash_item->setVisible(false); } else { - crash_item->setSize(5); + crash_item->setVisible(true); } addItem(crash_item); @@ -1415,9 +1415,9 @@ void MapWidget::showCrashPrediction(bool state) { show_crash_prediction_action->blockSignals(false); for(auto &itemManager: ac_items_managers) { if(state) { - itemManager->getCrashItem()->setSize(5*2); + itemManager->getCrashItem()->setVisible(true); } else { - itemManager->getCrashItem()->setSize(0); + itemManager->getCrashItem()->setVisible(false); } } }