diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index f71d89406443ad3d0d434cbf1162a6ff522d3f71..f538df61bfc73374721b5c646bab287f828d9414 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -93,7 +93,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); - emit valueChanged(uasId, "vis. time", pos.usec, time); + //emit valueChanged(uasId, "vis. time", pos.usec, time); emit valueChanged(uasId, "vis. roll", pos.roll, time); emit valueChanged(uasId, "vis. pitch", pos.pitch, time); emit valueChanged(uasId, "vis. yaw", pos.yaw, time); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e5efdea199a3693dba8e9d7673dcd764ba356335..8fbf860836afc94cb4a70cf14b387f70ce460353 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -379,7 +379,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_attitude_controller_output_t out; mavlink_msg_attitude_controller_output_decode(&message, &out); quint64 time = MG::TIME::getGroundTimeNowUsecs(); - emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time); + emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time); emit valueChanged(uasId, "att control roll", out.roll, time/1000.0f); emit valueChanged(uasId, "att control pitch", out.pitch, time/1000.0f); emit valueChanged(uasId, "att control yaw", out.yaw, time/1000.0f); @@ -390,7 +390,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_position_controller_output_t out; mavlink_msg_position_controller_output_decode(&message, &out); quint64 time = MG::TIME::getGroundTimeNowUsecs(); - emit attitudeThrustSetPointChanged(this, out.x, out.y, out.z, out.yaw, time); + emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time); emit valueChanged(uasId, "pos control x", out.x, time/1000.0f); emit valueChanged(uasId, "pos control y", out.y, time/1000.0f); emit valueChanged(uasId, "pos control z", out.z, time/1000.0f); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 4fe7572f1d86237fb314efc4022ca2c6232e8cd9..8747d9a7522a5555bdf595505e1b303005bfd0ca 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -302,6 +302,7 @@ signals: void heartbeat(UASInterface* uas); void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); + void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec); /** @brief Update the status of one satellite used for localization */ diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 38f7151d22dcce214f078ec16b24611c70c3f3ca..5320ecf283d19551205917e157143d30bf92f1af 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -83,7 +83,7 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m { qDebug() << "handleWaypointRequest"; - if (wpr->seq < 0) + if (wpr->seq > 0) { //TODO: send waypoint } diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 09509927964fd66a6f2765e3a36f70d98d1ae64e..3b97555aeb9a1ef5927d8066c76b2b1d08e7fdc9 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -141,6 +141,7 @@ QAction* CommConfigurationWindow::getAction() void CommConfigurationWindow::setLinkType(int linktype) { // Adjust the form layout per link type + Q_UNUSED(linktype); } void CommConfigurationWindow::setConnection() @@ -157,6 +158,7 @@ void CommConfigurationWindow::setConnection() void CommConfigurationWindow::setLinkName(QString name) { + Q_UNUSED(name); // FIXME action->setText(tr("Configure ") + link->getName()); action->setStatusTip(tr("Configure ") + link->getName()); this->window()->setWindowTitle(tr("Settings for ") + this->link->getName()); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 73b98b750f4d14643212bb53c32d31495e3ae89f..4452cf25d69019af3d415ea253fb651ba3719791 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -49,11 +49,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) : posXSet(0), posYSet(0), posZSet(0), - attXSaturation(0.33), - attYSaturation(0.33), - attYawSaturation(0.33), - posXSaturation(1.0), - posYSaturation(1.0), + attXSaturation(0.5f), + attYSaturation(0.5f), + attYawSaturation(0.5f), + posXSaturation(0.05), + posYSaturation(0.05), altitudeSaturation(1.0), lat(0), lon(0), @@ -62,9 +62,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) : x(0), y(0), z(0), + //yaw(0), localAvailable(0) { connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + refreshTimer->setInterval(80); } void HSIDisplay::paintEvent(QPaintEvent * event) @@ -88,14 +90,14 @@ void HSIDisplay::paintDisplay() const float margin = 0.1f; // 10% margin of total width on each side float baseRadius = (vwidth - vwidth * 2.0f * margin) / 2.0f; - quint64 refreshInterval = 100; - quint64 currTime = MG::TIME::getGroundTimeNow(); - if (currTime - lastPaintTime < refreshInterval) - { - // FIXME Need to find the source of the spurious paint events - //return; - } - lastPaintTime = currTime; +// quint64 refreshInterval = 100; +// quint64 currTime = MG::TIME::getGroundTimeNow(); +// if (currTime - lastPaintTime < refreshInterval) +// { +// // FIXME Need to find the source of the spurious paint events +// //return; +// } +// lastPaintTime = currTime; // Draw instruments // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW // Update scaling factor @@ -138,7 +140,7 @@ void HSIDisplay::paintDisplay() // Draw attitude QColor attitudeColor(200, 20, 20); - drawPositionSetpoint(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter); + drawAttitudeSetpoint(xCenterPos, yCenterPos, baseRadius, attitudeColor, &painter); @@ -196,6 +198,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); 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))); // Now connect the new UAS @@ -217,10 +220,11 @@ void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, altitudeSet = thrustDesired; } -void HSIDisplay::updatePositionSetpoints(int uasid, double xDesired, double yDesired, double zDesired, quint64 usec) +void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec) { Q_UNUSED(usec); Q_UNUSED(uasid); + Q_UNUSED(yawDesired); posXSet = xDesired; posYSet = yDesired; posZSet = zDesired; @@ -362,16 +366,21 @@ void HSIDisplay::drawPositionSetpoint(float xRef, float yRef, float radius, cons const float maxWidth = radius / 10.0f; const float minWidth = maxWidth * 0.3f; - float angle = asin(posXSet) + acos(posYSet); + float angle = atan2(posXSet, -posYSet); + angle -= M_PI/2.0f; QPolygonF p(6); - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); + //radius *= ((posXSaturation + posYSaturation) - sqrt(pow(posXSet, 2), pow(posYSet, 2))) / (2*posXSaturation); + + radius *= sqrt(pow(posXSet, 2) + pow(posYSet, 2)) / sqrt(posXSaturation + posYSaturation); + + p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); - p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.5f)); - p.replace(4, QPointF(xRef, yRef-radius * 0.46f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); + p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); + p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); @@ -382,6 +391,8 @@ void HSIDisplay::drawPositionSetpoint(float xRef, float yRef, float radius, cons painter->setPen(color); painter->setBrush(indexBrush); drawPolygon(p, painter); + + qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle; } void HSIDisplay::drawAttitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) @@ -390,16 +401,19 @@ void HSIDisplay::drawAttitudeSetpoint(float xRef, float yRef, float radius, cons const float maxWidth = radius / 10.0f; const float minWidth = maxWidth * 0.3f; - float angle = asin(attXSet) + acos(attYSet); + float angle = atan2(attXSet, attYSet); + angle -= M_PI/2.0f; + + radius *= sqrt(pow(attXSet, 2) + pow(attYSet, 2)) / sqrt(attXSaturation + attYSaturation); QPolygonF p(6); - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); + p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); - p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.5f)); - p.replace(4, QPointF(xRef, yRef-radius * 0.46f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f)); + p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); + p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); @@ -412,6 +426,8 @@ void HSIDisplay::drawAttitudeSetpoint(float xRef, float yRef, float radius, cons drawPolygon(p, painter); // TODO Draw Yaw indicator + + qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle; } void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index cd077a486a3586381a405ba16fa73a0cff08c20c..217ec8010b9636ddb6d6e261de86c2ac85295761 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -52,7 +52,7 @@ public slots: void setActiveUAS(UASInterface* uas); 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 updatePositionSetpoints(int uasid, double xDesired, double yDesired, double zDesired, quint64 usec); + 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); void paintEvent(QPaintEvent * event); diff --git a/src/ui/SerialConfigurationWindow.cc b/src/ui/SerialConfigurationWindow.cc index dff61a5c3e1e320f4ce3ac51b97f6bd81daf0098..087081fddb5c884350835ab200e63a563fb80404 100644 --- a/src/ui/SerialConfigurationWindow.cc +++ b/src/ui/SerialConfigurationWindow.cc @@ -502,6 +502,8 @@ void SerialConfigurationWindow::setPortName(QString port) void SerialConfigurationWindow::setLinkName(QString name) { + Q_UNUSED(name); + // FIXME action->setText(tr("Configure ") + link->getName()); action->setStatusTip(tr("Configure ") + link->getName()); setWindowTitle(tr("Configuration of ") + link->getName());