diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index c8269d75f6cb90cc477be5a07094d394a7738f22..e1ac0374d3058824a792d9f4dbc461fe10b2b6e2 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -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: diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 3c6ba920b2713ecedc13c8ae30217e6150e6b04e..71dc6d4c90bf57f5017374deb9a18dee254996db 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -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 */ diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 0e260bf1af20886478237cad56b3ae7cfa1641a4..20b8c09e7db0652c98e0cfb576a70172906eb313 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -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); diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index cdce6e7d911a7fae2324a4797f9e0fef4d18f9d4..fd815e705f935c0daf3f15f3bc7490818a58dc96 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -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); diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index f252420f6a2f5c79563895eb80b51916133fe174..e043af1598c6f2ea49ec1ceb147858c9311599ae 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -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(); + 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& 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::ref_ptr Pixhawk3DWidget::createTrail(const osg::Vec4& color) { - osg::ref_ptr geode(new osg::Geode()); osg::ref_ptr geometry(new osg::Geometry()); geometry->setUseDisplayList(false); - geode->addDrawable(geometry.get()); osg::ref_ptr 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 @@ -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 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(geometry->getPrimitiveSet(0)); + + osg::ref_ptr vertices(new osg::Vec3Array); + + const QVarLengthArray& 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(geometry->getPrimitiveSet(0)); - osg::ref_ptr 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 diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index c5bde68114694d65057fd67c391334f58dc7945a..117a9a7915682e78696bbe9fa74b3ca5d4b11133 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -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 createLocalGrid(void); osg::ref_ptr createWorldGrid(void); - osg::ref_ptr createTrail(const osg::Vec4& color); + osg::ref_ptr createTrail(const osg::Vec4& color); osg::ref_ptr createMap(void); osg::ref_ptr createRGBD3D(void); osg::ref_ptr 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 trail; + QMap > trails; + QMap trailDrawableIdxs; osg::ref_ptr vehicleModel; osg::ref_ptr hudBackgroundGeometry;