Many improvements to HSI

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);

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);

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 */

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
         }

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());

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)

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);

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());