Skip to content

Commit

Permalink
Merge pull request #24 from EwoudSmeur/crash_prediction
Browse files Browse the repository at this point in the history 
predict crash location on GCS
  • Loading branch information
@Fabien-B
Fabien-B authored Feb 21, 2024
2 parents c1ae3a0 + 14eebfd commit 4df9963
Show file tree
Hide file tree
Showing 7 changed files with 113 additions and 14 deletions.
9 changes: 7 additions & 2 deletions src/widgets/map/acitemmanager.cpp
View file
Original file line number Diff line number Diff line change
@@ -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)
{

}
Expand Down Expand Up @@ -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;
}
}
4 changes: 3 additions & 1 deletion src/widgets/map/acitemmanager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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*);
Expand All @@ -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:
Expand All @@ -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;
};
Expand Down
1 change: 1 addition & 0 deletions src/widgets/map/graphics_objects/graphics_object.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ class GraphicsObject : public QObject
CARROT,
GCS,
DCSHOT,
CRASH,
};

enum Animation {
Expand Down
7 changes: 7 additions & 0 deletions src/widgets/map/graphics_objects/graphics_point.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

}
Expand Down
3 changes: 3 additions & 0 deletions src/widgets/map/graphics_objects/graphics_text.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,9 @@ void GraphicsText::changeFocus() {
case DCSHOT:
setVisible(false);
break;
case CRASH:
setVisible(false);
break;
}

update();
Expand Down
99 changes: 88 additions & 11 deletions src/widgets/map/mapwidget.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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();
Expand All @@ -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);
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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);
Expand All @@ -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, [=](){
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -1228,7 +1292,7 @@ void MapWidget::setAcArrowSize(int s) {
}

void MapWidget::onGVF(QString sender, pprzlink::Message msg) {

if(!gvf_loaded) {
return;
}
Expand All @@ -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);
}
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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);
}
}
}
4 changes: 4 additions & 0 deletions src/widgets/map/mapwidget.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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

0 comments on commit 4df9963

Please sign in to comment.