Supported multiple attitude estimates in HUD

src/uas/UAS.cc

@@ -440,7 +440,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f)
                     && (lpVoltage < tickVoltage))
             {
-                GAudioOutput::instance()->say(QString("voltage warning: %1 volt").arg(lpVoltage, 2));
+                GAudioOutput::instance()->say(QString("voltage warning: %1 volt").arg(lpVoltage, 2, 'f', 0, QChar(' ')));
                 lastTickVoltageValue = tickLowpassVoltage;
             }
 
@@ -471,9 +471,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 stopLowBattAlarm();
             }
 
-            qDebug() << "START" << startVoltage;
-
-
             // control_sensors_enabled:
             // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
             emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11));

src/ui/HUD.cc

@@ -256,6 +256,7 @@ void HUD::setActiveUAS(UASInterface* uas)
     if (this->uas != NULL) {
         // Disconnect any previously connected active MAV
         disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
+        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
         disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
         disconnect(this->uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
         disconnect(this->uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
@@ -278,6 +279,7 @@ void HUD::setActiveUAS(UASInterface* uas)
         // Now connect the new UAS
         // Setup communication
         connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
+        connect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
         connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
         connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
         connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
@@ -317,6 +319,13 @@ void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double ya
     this->yaw = yaw;
 }
 
+void HUD::updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp)
+{
+    Q_UNUSED(uas);
+    Q_UNUSED(timestamp);
+    attitudes.insert(component, QVector3D(roll, pitch, yaw));
+}
+
 void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
 {
     Q_UNUSED(uas);
@@ -816,16 +825,37 @@ void HUD::paintHUD()
 
             painter.translate(refToScreenX(yawTrans), 0);
 
-            // Rotate view and draw all roll-dependent indicators
-            painter.rotate((rollLP/M_PI)* -180.0f);
+            // Old single-component pitch drawing
+//            // Rotate view and draw all roll-dependent indicators
+//            painter.rotate((rollLP/M_PI)* -180.0f);
+
+//            painter.translate(0, (-pitchLP/(float)M_PI)* -180.0f * refToScreenY(1.8f));
+
+//            //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
 
-            painter.translate(0, (-pitchLP/(float)M_PI)* -180.0f * refToScreenY(1.8f));
+//            // PITCH
 
-            //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
+//            paintPitchLines(pitchLP, &painter);
 
-            // PITCH
+            QColor attColor = painter.pen().color();
+
+            // Draw multi-component attitude
+            foreach (QVector3D att, attitudes.values())
+            {
+                attColor = attColor.darker(200);
+                painter.setPen(attColor);
+                // Rotate view and draw all roll-dependent indicators
+                painter.rotate((att.x()/M_PI)* -180.0f);
+
+                painter.translate(0, (-att.y()/(float)M_PI)* -180.0f * refToScreenY(1.8f));
+
+                //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
+
+                // PITCH
+
+                paintPitchLines(att.y(), &painter);
+            }
 
-            paintPitchLines(pitchLP, &painter);
             painter.end();
         } else {
             QPainter painter;

src/ui/HUD.h

@@ -37,6 +37,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QPainter>
 #include <QFontDatabase>
 #include <QTimer>
+#include <QVector3D>
 #include "UASInterface.h"
 
 /**
@@ -63,7 +64,10 @@ public slots:
     /** @brief Set the currently monitored UAS */
     virtual void setActiveUAS(UASInterface* uas);
 
+    /** @brief Attitude from main autopilot / system state */
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
+    /** @brief Attitude from one specific component / redundant autopilot */
+    void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
 //    void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
     void updateBattery(UASInterface*, double, double, int);
     void receiveHeartbeat(UASInterface*);
@@ -193,6 +197,7 @@ protected:
     float roll;
     float pitch;
     float yaw;
+    QMap<uint8_t, QVector3D> attitudes;
     float rollLP;
     float pitchLP;
     float yawLP;