diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 6c800f7..78b1c57 100644 --- a/src/uas/UAS.cc +++ b/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)); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 90898ee..6bf0822 100644 --- a/src/ui/HUD.cc +++ b/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; diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 5239808..d9617f4 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -37,6 +37,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #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 attitudes; float rollLP; float pitchLP; float yawLP;