Skip to content

Q
qgroundcontrol
Commit 316f6194 authored by pixhawk's avatar pixhawk
Browse files
Options

Working on multi-component, multi-path visualization

parent 6107c889
Hide whitespace changes
Inline Side-by-side
Showing with 170 additions and 81 deletions
src/uas/UAS.cc
View file @ 316f6194
......@@ -240,9 +240,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
bool multiComponentSourceDetected = false;
bool wrongComponent = false;
switch (message.compid)
{
case MAV_COMP_ID_IMU_2:
// Prefer IMU 2 over IMU 1 (FIXME)
componentID[message.msgid] = MAV_COMP_ID_IMU_2;
break;
default:
// Do nothing
break;
}
// Store component ID
if (componentID[message.msgid] == -1)
{
// Prefer the first component
componentID[message.msgid] = message.compid;
}
else
......@@ -488,19 +500,38 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_local_position_ned_t pos;
mavlink_msg_local_position_ned_decode(&message, &pos);
quint64 time = getUnixTime(pos.time_boot_ms);
localX = pos.x;
localY = pos.y;
localZ = pos.z;
emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
// Set internal state
if (!positionLock) {
// If position was not locked before, notify positive
GAudioOutput::instance()->notifyPositive();
// Emit position always with component ID
emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
if (!wrongComponent)
{
localX = pos.x;
localY = pos.y;
localZ = pos.z;
// Emit
emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);
// Set internal state
if (!positionLock) {
// If position was not locked before, notify positive
GAudioOutput::instance()->notifyPositive();
}
positionLock = true;
isLocalPositionKnown = true;
}
positionLock = true;
isLocalPositionKnown = true;
}
break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
{
mavlink_global_vision_position_estimate_t pos;
mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
}
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
......@@ -785,6 +816,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
}
break;
case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
{
mavlink_set_local_position_setpoint_t p;
mavlink_msg_set_local_position_setpoint_decode(&message, &p);
emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw);
}
break;
case MAVLINK_MSG_ID_STATUSTEXT:
{
QByteArray b;
......@@ -954,7 +992,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
#endif
// Messages to ignore
case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
case MAVLINK_MSG_ID_RAW_IMU:
case MAVLINK_MSG_ID_SCALED_IMU:
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
......
src/uas/UASInterface.h
View file @ 316f6194
......@@ -449,8 +449,12 @@ signals:
void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
void attitudeSpeedChanged(int uas, double rollspeed, double pitchspeed, double yawspeed, quint64 usec);
void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
/** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
/** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
void userPositionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec);
void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec);
void altitudeChanged(int uasid, double altitude);
/** @brief Update the status of one satellite used for localization */
......
src/ui/HSIDisplay.cc
View file @ 316f6194
......@@ -668,6 +668,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
......@@ -688,6 +689,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
......@@ -811,6 +813,16 @@ void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, do
this->yaw = yaw;
}
void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
{
uiXSetCoordinate = xDesired;
uiYSetCoordinate = yDesired;
uiZSetCoordinate = zDesired;
uiYawSet = yawDesired;
userXYSetPointSet = true;
userSetPointSet = true;
}
void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec)
{
Q_UNUSED(usec);
......
src/ui/HSIDisplay.h
View file @ 316f6194
......@@ -57,6 +57,7 @@ public slots:
void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
void updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec);
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 316f6194
......@@ -58,7 +58,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
, selectedWpIndex(-1)
, displayLocalGrid(false)
, displayWorldGrid(true)
, displayTrail(true)
, displayTrails(true)
, displayImagery(true)
, displayWaypoints(true)
, displayRGBD2D(false)
......@@ -88,7 +88,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
allocentricMap->addChild(worldGridNode);
// generate empty trail model
trailNode = createTrail(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
trailNode = new osg::Geode;
rollingMap->addChild(trailNode);
// generate map model
......@@ -114,7 +114,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
rollingMap->addChild(obstacleGroupNode);
// generate path model
pathNode = createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f));
pathNode = new osg::Geode;
pathNode->addDrawable(createTrail(osg::Vec4(1.0f, 0.8f, 0.0f, 1.0f)));
rollingMap->addChild(pathNode);
#endif
......@@ -143,15 +144,74 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
void
Pixhawk3DWidget::setActiveUAS(UASInterface* uas)
{
if (this->uas != NULL && this->uas != uas)
if (this->uas == uas)
{
return;
}
if (this->uas != NULL)
{
// Disconnect any previously connected active MAV
//disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
}
connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(addToTrails(UASInterface*,int,double,double,double,quint64)));
trails.clear();
this->uas = uas;
}
void
Pixhawk3DWidget::addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time)
{
if (this->uas->getUASID() != uas->getUASID())
{
return;
}
if (!trails.contains(component))
{
trails[component] = QVarLengthArray<osg::Vec3d, 10000>();
trailDrawableIdxs[component] = trails.size() - 1;
trailNode->addDrawable(createTrail(osg::Vec4((float)qrand() / RAND_MAX,
(float)qrand() / RAND_MAX,
(float)qrand() / RAND_MAX,
1.0)));
}
QVarLengthArray<osg::Vec3d, 10000>& trail = trails[component];
bool addToTrail = false;
if (trail.size() > 0)
{
if (fabs(x - trail[trail.size() - 1].x()) > 0.01f ||
fabs(y - trail[trail.size() - 1].y()) > 0.01f ||
fabs(z - trail[trail.size() - 1].z()) > 0.01f)
{
addToTrail = true;
}
}
else
{
addToTrail = true;
}
if (addToTrail)
{
osg::Vec3d p(x, y, z);
if (trail.size() == trail.capacity())
{
memcpy(trail.data(), trail.data() + 1,
(trail.size() - 1) * sizeof(osg::Vec3d));
trail[trail.size() - 1] = p;
}
else
{
trail.append(p);
}
}
}
void
Pixhawk3DWidget::selectFrame(QString text)
{
......@@ -196,20 +256,20 @@ Pixhawk3DWidget::showWorldGrid(int32_t state)
}
void
Pixhawk3DWidget::showTrail(int32_t state)
Pixhawk3DWidget::showTrails(int32_t state)
{
if (state == Qt::Checked)
{
if (!displayTrail)
if (!displayTrails)
{
trail.clear();
trails.clear();
}
displayTrail = true;
displayTrails = true;
}
else
{
displayTrail = false;
displayTrails = false;
}
}
......@@ -638,8 +698,8 @@ Pixhawk3DWidget::buildLayout(void)
worldGridCheckBox->setChecked(displayWorldGrid);
QCheckBox* trailCheckBox = new QCheckBox(this);
trailCheckBox->setText("Trail");
trailCheckBox->setChecked(displayTrail);
trailCheckBox->setText("Trails");
trailCheckBox->setChecked(displayTrails);
QCheckBox* waypointsCheckBox = new QCheckBox(this);
waypointsCheckBox->setText("Waypoints");
......@@ -693,7 +753,7 @@ Pixhawk3DWidget::buildLayout(void)
connect(worldGridCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showWorldGrid(int)));
connect(trailCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showTrail(int)));
this, SLOT(showTrails(int)));
connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showWaypoints(int)));
connect(mapComboBox, SIGNAL(currentIndexChanged(int)),
......@@ -719,7 +779,7 @@ Pixhawk3DWidget::display(void)
// set node visibility
allocentricMap->setChildValue(worldGridNode, displayWorldGrid);
rollingMap->setChildValue(localGridNode, displayLocalGrid);
rollingMap->setChildValue(trailNode, displayTrail);
rollingMap->setChildValue(trailNode, displayTrails);
rollingMap->setChildValue(mapNode, displayImagery);
rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
rollingMap->setChildValue(targetNode, enableTarget);
......@@ -763,9 +823,9 @@ Pixhawk3DWidget::display(void)
robotPitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
robotRoll, osg::Vec3d(0.0f, 1.0f, 0.0f)));
if (displayTrail)
if (displayTrails)
{
updateTrail(robotX, robotY, robotZ);
updateTrails(robotX, robotY, robotZ);
}
if (frame == MAV_FRAME_GLOBAL && displayImagery)
......@@ -1157,13 +1217,11 @@ Pixhawk3DWidget::createWorldGrid(void)
return geode;
}
osg::ref_ptr<osg::Geode>
osg::ref_ptr<osg::Geometry>
Pixhawk3DWidget::createTrail(const osg::Vec4& color)
{
osg::ref_ptr<osg::Geode> geode(new osg::Geode());
osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
geometry->setUseDisplayList(false);
geode->addDrawable(geometry.get());
osg::ref_ptr<osg::Vec3dArray> vertices(new osg::Vec3dArray());
geometry->setVertexArray(vertices);
......@@ -1183,7 +1241,7 @@ Pixhawk3DWidget::createTrail(const osg::Vec4& color)
stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
geometry->setStateSet(stateset);
return geode;
return geometry;
}
osg::ref_ptr<Imagery>
......@@ -1374,58 +1432,33 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
}
void
Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ)
{
if (robotX == 0.0f || robotY == 0.0f || robotZ == 0.0f)
{
return;
}
QMapIterator<int,int> it(trailDrawableIdxs);
bool addToTrail = false;
if (trail.size() > 0)
{
if (fabs(robotX - trail[trail.size() - 1].x()) > 0.01f ||
fabs(robotY - trail[trail.size() - 1].y()) > 0.01f ||
fabs(robotZ - trail[trail.size() - 1].z()) > 0.01f)
{
addToTrail = true;
}
}
else
while (it.hasNext())
{
addToTrail = true;
}
it.next();
if (addToTrail)
{
osg::Vec3d p(robotX, robotY, robotZ);
if (trail.size() == trail.capacity())
{
memcpy(trail.data(), trail.data() + 1,
(trail.size() - 1) * sizeof(osg::Vec3d));
trail[trail.size() - 1] = p;
}
else
osg::Geometry* geometry = trailNode->getDrawable(it.value())->asGeometry();
osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
const QVarLengthArray<osg::Vec3d, 10000>& trail = trails.value(it.key());
for (int i = 0; i < trail.size(); ++i)
{
trail.append(p);
vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
trail[i].x() - robotX,
-(trail[i].z() - robotZ)));
}
}
osg::Geometry* geometry = trailNode->getDrawable(0)->asGeometry();
osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
for (int i = 0; i < trail.size(); ++i)
{
vertices->push_back(osg::Vec3d(trail[i].y() - robotY,
trail[i].x() - robotX,
-(trail[i].z() - robotZ)));
geometry->setVertexArray(vertices);
drawArrays->setFirst(0);
drawArrays->setCount(vertices->size());
geometry->dirtyBound();
}
geometry->setVertexArray(vertices);
drawArrays->setFirst(0);
drawArrays->setCount(vertices->size());
geometry->dirtyBound();
}
void
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ 316f6194
......@@ -59,12 +59,13 @@ public:
public slots:
void setActiveUAS(UASInterface* uas);
void addToTrails(UASInterface* uas, int component, double x, double y, double z, quint64 time);
private slots:
void selectFrame(QString text);
void showLocalGrid(int state);
void showWorldGrid(int state);
void showTrail(int state);
void showTrails(int state);
void showWaypoints(int state);
void selectMapSource(int index);
void selectVehicleModel(int index);
......@@ -109,7 +110,7 @@ private:
osg::ref_ptr<osg::Geode> createLocalGrid(void);
osg::ref_ptr<osg::Geode> createWorldGrid(void);
osg::ref_ptr<osg::Geode> createTrail(const osg::Vec4& color);
osg::ref_ptr<osg::Geometry> createTrail(const osg::Vec4& color);
osg::ref_ptr<Imagery> createMap(void);
osg::ref_ptr<osg::Geode> createRGBD3D(void);
osg::ref_ptr<osg::Node> createTarget(void);
......@@ -120,7 +121,7 @@ private:
void updateHUD(double robotX, double robotY, double robotZ,
double robotRoll, double robotPitch, double robotYaw,
const QString& utmZone);
void updateTrail(double robotX, double robotY, double robotZ);
void updateTrails(double robotX, double robotY, double robotZ);
void updateImagery(double originX, double originY, double originZ,
const QString& zone);
void updateWaypoints(void);
......@@ -149,7 +150,7 @@ private:
bool displayLocalGrid;
bool displayWorldGrid;
bool displayTrail;
bool displayTrails;
bool displayImagery;
bool displayWaypoints;
bool displayRGBD2D;
......@@ -161,7 +162,8 @@ private:
bool followCamera;
QVarLengthArray<osg::Vec3d, 10000> trail;
QMap<int, QVarLengthArray<osg::Vec3d, 10000> > trails;
QMap<int, int> trailDrawableIdxs;
osg::ref_ptr<osg::Node> vehicleModel;
osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment