Commit dfae554b authored by pixhawk's avatar pixhawk

Major stability improvements

parent 4fa6562e
This source diff could not be displayed because it is too large. You can view the blob instead.
QWidget#colorIcon {}
QWidget {
/*background-color: #252528;
color: #DDDDDF;
border-color: #EEEEEE;*/
background-clip: border;
font-size: 11px;
}
QGroupBox {
border: 1px solid;/* #555555; */
border-radius: 3px;
padding: 10px 0px 0px 0px;
margin-top: 1ex; /* leave space at the top for the title */
}
/*
QCheckBox {
background-color: #252528;
color: #454545;
}*/
QGroupBox::title {
subcontrol-origin: margin;
subcontrol-position: top center; /* position at the top center */
margin: 0 3px 0px 3px;
padding: 0 3px 0px 0px;
font: bold 8px;
}
QDockWidget {
font: bold;
border: 1px solid #32345E;
}
QDockWidget::close-button, QDockWidget::float-button {
background-color: #181820;
color: #EEEEEE;
}
QDockWidget::title {
text-align: left;
background: #222224;
padding-left: 5px;
height: 10px;
border-bottom: 1px solid #555555;
}
QSeparator {
color: #EEEEEE;
}
QSpinBox {
min-height: 12 px;
}
QPushButton {
font-weight: bold;
min-height: 22px;
max-height: 32px;
border: 1px solid #AAAAAA;
border-radius: 5px;
padding-left: 10px;
padding-right: 10px;
/*background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #303030, stop: 1 #202020);*/
}
/*
QPushButton:checked {
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #101010, stop: 1 #404040);
}
QPushButton:pressed {
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #bbbbbb, stop: 1 #b0b0b0);
}*/
QToolButton {
font-weight: bold;
min-height: 16px;
min-width: 24px;
max-height: 32px;
border: 1px solid #EEEEEE;
border-radius: 5px;
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #303030, stop: 1 #202020);
}
QToolButton:pressed {
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #bbbbbb, stop: 1 #b0b0b0);
}
QPushButton#forceLandButton {
font-weight: bold;
min-height: 30px;
color: #000000;
background: qlineargradient(x1:0, y1:0, x2:0, y2:1,
stop:0 #ffee01, stop:1 #ae8f00) url("ICONDIR/control/emergency-button.png");
background-clip: border;
border-width: 1px;
border-color: #555555;
border-radius: 5px;
}
QPushButton:pressed#forceLandButton {
font-weight: bold;
min-height: 30px;
color: #000000;
background: qlineargradient(x1:0, y1:0, x2:0, y2:1,
stop:0 #bbaa00, stop:1 #a05b00) url("ICONDIR/control/emergency-button.png");
background-clip: border;
border-width: 1px;
border-color: #555555;
border-radius: 5px;
}
QPushButton#killButton {
font-weight: bold;
min-height: 30px;
color: #000000;
background: qlineargradient(x1:0, y1:0, x2:0, y2:1,
stop:0 #ffb917, stop:1 #b37300) url("ICONDIR/control/emergency-button.png");
background-clip: border;
border-width: 1px;
border-color: #555555;
border-radius: 5px;
}
QPushButton:pressed#killButton {
font-weight: bold;
min-height: 30px;
color: #000000;
background: qlineargradient(x1:0, y1:0, x2:0, y2:1,
stop:0 #bb8500, stop:1 #903000) url("ICONDIR/control/emergency-button.png");
background-clip: border;
border-width: 1px;
border-color: #555555;
border-radius: 5px;
}
QPushButton#autoButton {
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #4eb0e8, stop: 1 #285a77);
}
QPushButton:checked#autoButton {
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #a77f11, stop: 1 #f1b718);
}
QProgressBar {
border: 1px solid white;
border-radius: 4px;
text-align: center;
padding: 2px;
color: white;
background-color: #111111;
}
QProgressBar:horizontal {
height 12px;
}
QProgressBar:vertical {
width 12px;
}
QProgressBar::chunk {
background-color: #656565;
}
QProgressBar::chunk#batteryBar {
background-color: green;
}
QProgressBar::chunk#speedBar {
background-color: yellow;
}
QProgressBar::chunk#thrustBar {
background-color: orange;
}
QProgressBar::chunk#bandwidthBar {
background-color: orange;
}
QProgressBar::chunk#loadBar {
background-color: yellow;
}
QProgressBar::chunk#topRotorBar {
background-color: yellow;
}
QProgressBar::chunk#botRotorBar {
background-color: yellow;
}
QProgressBar::chunk#leftServoBar {
background-color: #99BFDD;
}
QProgressBar::chunk#rightServoBar
{
background-color: blue;
}
QWidget#colorIcon {}
QWidget {
background-color: #151518;
background-color: #050508;
color: #DDDDDF;
background-clip: border;
font-size: 11px;
}
QGroupBox {
border: 1px solid #777777;
border: 1px solid #66666B;
border-radius: 3px;
padding: 10px 0px 0px 0px;
margin-top: 1ex; /* leave space at the top for the title */
......@@ -107,11 +107,11 @@ QPushButton {
font-weight: bold;
min-height: 18px;
max-height: 32px;
border: 1px solid #EEEEEE;
border: 2px solid #4A4A4F;
border-radius: 5px;
padding-left: 10px;
padding-right: 10px;
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #303030, stop: 1 #202020);
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208);
}
QPushButton:checked {
......@@ -186,7 +186,7 @@ QPushButton:pressed#killButton {
QPushButton#controlButton {
min-height: 25px;
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #b76f11, stop: 1 #e1a718);
background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #A0AE00, stop: 1 #909E00);
}
QPushButton:checked#controlButton {
......@@ -194,12 +194,12 @@ QPushButton:checked#controlButton {
}
QProgressBar {
border: 1px solid white;
border: 1px solid #EEEEEE;
border-radius: 4px;
text-align: center;
padding: 2px;
color: white;
background-color: #111111;
background-color: #111118;
}
QProgressBar:horizontal {
......
......@@ -78,7 +78,7 @@ HEADERS += src/MG.h \
src/configuration.h \
src/ui/uas/UASView.h \
src/ui/CameraView.h \
src/comm/MavlinkSimulationLink.h \
src/comm/MAVLinkSimulationLink.h \
src/comm/UDPLink.h \
src/ui/ParameterInterface.h \
src/ui/WaypointList.h \
......@@ -129,7 +129,7 @@ SOURCES += src/main.cc \
src/ui/linechart/ScrollZoomer.cc \
src/ui/uas/UASView.cc \
src/ui/CameraView.cc \
src/comm/MavlinkSimulationLink.cc \
src/comm/MAVLinkSimulationLink.cc \
src/comm/UDPLink.cc \
src/ui/ParameterInterface.cc \
src/ui/WaypointList.cc \
......
This diff is collapsed.
......@@ -38,12 +38,14 @@ This file is part of the PIXHAWK project
#include <QQueue>
#include <QMutex>
#include <inttypes.h>
#include <LinkInterface.h>
#include <mavlink.h>
#include "LinkInterface.h"
class MAVLinkSimulationLink : public LinkInterface
{
Q_OBJECT
public:
MAVLinkSimulationLink(QFile* readFile=NULL, QFile* writeFile=NULL, int rate=5);
MAVLinkSimulationLink(QString readFile="", QString writeFile="", int rate=5);
~MAVLinkSimulationLink();
bool isConnected();
qint64 bytesAvailable();
......@@ -94,7 +96,6 @@ protected:
/** File which contains the input data (simulated robot messages) **/
QFile* simulationFile;
QString simulationHeader;
int lineCounter = 0;
/** File where the commands sent by the groundstation are stored **/
QFile* receiveFile;
QTextStream stream;
......@@ -112,9 +113,15 @@ protected:
int id;
QString name;
qint64 timeOffset;
sys_status_t status;
void setMaximumTimeNoise(int milliseconds);
void addTimeNoise();
void enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg);
signals:
void valueChanged(int uasId, QString curve, double value, quint64 usec);
};
......
......@@ -113,6 +113,50 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit statusChanged(this, uasState, stateDescription);
onboardTimeOffset = 0; // Reset offset measurement
break;
case MAVLINK_MSG_ID_SYS_STATUS:
{
sys_status_t state;
message_sys_status_decode(&message, &state);
getStatusForCode((int)state.status, uasState, stateDescription);
emit statusChanged(this, uasState, stateDescription);
QString mode;
switch (state.mode)
{
case MAV_MODE_LOCKED:
mode = "MAV_MODE_LOCKED";
break;
case MAV_MODE_MANUAL:
mode = "MAV_MODE_MANUAL";
break;
case MAV_MODE_AUTO:
mode = "MAV_MODE_AUTO";
break;
case MAV_MODE_TEST1:
mode = "MAV_MODE_TEST1";
break;
case MAV_MODE_TEST2:
mode = "MAV_MODE_TEST2";
break;
case MAV_MODE_TEST3:
mode = "MAV_MODE_TEST3";
break;
default:
mode = "MAV_MODE_UNINIT";
break;
}
emit modeChanged(this->getUASID(), mode, "");
currentVoltage = state.vbat;
filterVoltage(currentVoltage);
if (startVoltage == 0) startVoltage = currentVoltage;
timeRemaining = calculateTimeRemaining();
//qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
emit batteryChanged(this, filterVoltage(), getChargeLevel(), timeRemaining);
emit voltageChanged(message.sysid, state.vbat/1000.0f);
}
break;
/*
case MAVLINK_MSG_ID_SYSTEM:
// std::cerr << std::endl;
......@@ -198,9 +242,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
//std::cerr << "BYTES RESSOURCE RECEIVED" << std::endl;
emit imageDataReceived(message_bytestream_get_rid(message.payload), message_bytestream_get_data(message.payload), message_bytestream_get_length(message.payload), message_bytestream_get_index(message.payload));
break;
case MAVLINK_MSG_ID_SENSRAW:
*/
case MAVLINK_MSG_ID_RAW_IMU:
{
quint64 time = message_sensraw_get_usec(message.payload);
raw_imu_t raw;
message_raw_imu_decode(&message, &raw);
quint64 time = raw.msec;
if (time == 0)
{
time = MG::TIME::getGroundTimeNow();
......@@ -214,21 +261,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
time += onboardTimeOffset;
}
emit valueChanged(uasId, "Accel. X", message_sensraw_get_xacc(message.payload), time);
emit valueChanged(uasId, "Accel. Y", message_sensraw_get_yacc(message.payload), time);
emit valueChanged(uasId, "Accel. Z", message_sensraw_get_zacc(message.payload), time);
emit valueChanged(uasId, "Gyro Phi", message_sensraw_get_xgyro(message.payload), time);
emit valueChanged(uasId, "Gyro Theta", message_sensraw_get_ygyro(message.payload), time);
emit valueChanged(uasId, "Gyro Psi", message_sensraw_get_zgyro(message.payload), time);
emit valueChanged(uasId, "Mag. X", message_sensraw_get_xmag(message.payload), time);
emit valueChanged(uasId, "Mag. Y", message_sensraw_get_ymag(message.payload), time);
emit valueChanged(uasId, "Mag. Z", message_sensraw_get_zmag(message.payload), time);
emit valueChanged(uasId, "Ground Dist.", message_sensraw_get_gdist(message.payload), time);
emit valueChanged(uasId, "Pressure", message_sensraw_get_baro(message.payload), time);
emit valueChanged(uasId, "Temperature", message_sensraw_get_temp(message.payload), time);
emit valueChanged(uasId, "Accel. X", raw.xacc, time);
emit valueChanged(uasId, "Accel. Y", raw.yacc, time);
emit valueChanged(uasId, "Accel. Z", raw.zacc, time);
emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time);
emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time);
emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time);
emit valueChanged(uasId, "Mag. X", raw.xmag, time);
emit valueChanged(uasId, "Mag. Y", raw.ymag, time);
emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
}
break;
*/
case MAVLINK_MSG_ID_ATTITUDE:
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << message_attitude_get_roll(message.payload) << " pitch: " << message_attitude_get_pitch(message.payload) << " yaw: " << message_attitude_get_yaw(message.payload) << std::endl;
......@@ -338,12 +381,15 @@ void UAS::setAutoMode(bool autoMode)
}
}
void UAS::setMode(enum MAV_MODE mode)
void UAS::setMode(int mode)
{
this->mode = mode;
mavlink_message_t msg;
message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode);
sendMessage(msg);
if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
{
this->mode = mode;
mavlink_message_t msg;
message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode);
sendMessage(msg);
}
}
void UAS::sendMessage(mavlink_message_t message)
......
......@@ -115,7 +115,7 @@ protected:
bool controlPitchManual; ///< status flag, true if pitch is controlled manually
bool controlYawManual; ///< status flag, true if yaw is controlled manually
bool controlThrustManual;///< status flag, true if thrust is controlled manually
enum MAV_MODE mode; ///< The current mode of the MAV
int mode; ///< The current mode of the MAV
quint64 onboardTimeOffset;
/** @brief Set the current battery type */
......@@ -178,7 +178,7 @@ public slots:
void setSelected();
/** @brief Set current mode of operation, e.g. auto or manual */
void setMode(enum MAV_MODE mode);
void setMode(int mode);
signals:
......
/*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
Please see our website at <http://pixhawk.ethz.ch>
(c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
......@@ -159,6 +158,8 @@ public slots:
* @param autoMode true for autonomous operation, false for manual control
*/
virtual void setAutoMode(bool autoMode) = 0;
virtual void setMode(int mode) = 0;
/** Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/
virtual void emergencySTOP() = 0;
/** Kills the robot. All systems are immediately shut down (e.g. the main power line is cut). This might lead to a crash **/
......@@ -210,6 +211,8 @@ signals:
* @param description longer textual description. Should be however limited to a short text, e.g. 200 chars.
*/
void statusChanged(UASInterface* uas, QString status, QString description);
/** @brief Robot mode has changed */
void modeChanged(int sysId, QString status, QString description);
/** @brief A command has been issued **/
void commandSent(int command);
/** @brief The connection status has changed **/
......
......@@ -31,13 +31,14 @@ This file is part of the PIXHAWK project
#include <QFile>
#include <QGLWidget>
#include <QStringList>
#include "UASManager.h"
#include "HDDisplay.h"
#include "ui_HDDisplay.h"
#include <QDebug>
HDDisplay::HDDisplay(QWidget *parent) :
HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) :
QWidget(parent),
uas(NULL),
values(QMap<QString, float>()),
......@@ -45,6 +46,10 @@ HDDisplay::HDDisplay(QWidget *parent) :
valuesMean(QMap<QString, float>()),
valuesCount(QMap<QString, int>()),
lastUpdate(QMap<QString, quint64>()),
minValues(),
maxValues(),
goodRanges(),
critRanges(),
xCenterOffset(0.0f),
yCenterOffset(0.0f),
vwidth(80.0f),
......@@ -62,6 +67,7 @@ HDDisplay::HDDisplay(QWidget *parent) :
strongStrokeWidth(1.5f),
normalStrokeWidth(1.0f),
fineStrokeWidth(0.5f),
acceptList(plotList),
m_ui(new Ui::HDDisplay)
{
//m_ui->setupUi(this);
......@@ -82,7 +88,7 @@ HDDisplay::HDDisplay(QWidget *parent) :
if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName;
// Connect with UAS
//connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
//start();
}
......@@ -93,7 +99,6 @@ HDDisplay::~HDDisplay()
void HDDisplay::paintEvent(QPaintEvent * event)
{
paintGL();
}
......@@ -108,13 +113,36 @@ void HDDisplay::paintGL()
if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH;
QPainter painter(this);
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
painter.fillRect(QRect(0, 0, width(), height()), backgroundColor);
painter.setRenderHint(QPainter::HighQualityAntialiasing);
const float gaugeWidth = 15.0f;
const int columns = 3;
const float spacing = 0.4f; // 40% of width
const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.1f));
const QColor gaugeColor = QColor(200, 200, 200);
drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter);
drawGauge(10.0f, 10.0f, gaugeWidth/2.0f, 0, 1.0f, "thrust", values.value("thrust", 0.0f), gaugeColor, &painter, qMakePair(0.45f, 0.8f), qMakePair(0.8f, 1.0f), true);
drawGauge(10.0f+gaugeWidth*1.7f, 10.0f, gaugeWidth/2.0f, 0, 10.0f, "altitude", values.value("altitude", 0.0f), gaugeColor, &painter, qMakePair(1.0f, 2.5f), qMakePair(0.0f, 0.5f), true);
//drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter);
//drawGauge(15.0f, 15.0f, gaugeWidth/2.0f, 0, 1.0f, "thrust", values.value("thrust", 0.0f), gaugeColor, &painter, qMakePair(0.45f, 0.8f), qMakePair(0.8f, 1.0f), true);
//drawGauge(15.0f+gaugeWidth*1.7f, 15.0f, gaugeWidth/2.0f, 0, 10.0f, "altitude", values.value("altitude", 0.0f), gaugeColor, &painter, qMakePair(1.0f, 2.5f), qMakePair(0.0f, 0.5f), true);
// Left spacing from border / other gauges, measured from left edge to center
float leftSpacing = gaugeWidth * spacing;
float xCoord = leftSpacing + gaugeWidth/2.0f;
float topSpacing = leftSpacing;
float yCoord = topSpacing + gaugeWidth/2.0f;
for (int i = 0; i < acceptList->size(); ++i)
{
QString value = acceptList->at(i);
drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, 0.0f), maxValues.value(value, 1.0f), value, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true);
xCoord += gaugeWidth + leftSpacing;
// Move one row down if necessary
if (xCoord + gaugeWidth > vwidth)
{
yCoord += topSpacing + gaugeWidth;
xCoord = leftSpacing + gaugeWidth/2.0f;
}
}
}
void HDDisplay::start()
......@@ -133,7 +161,7 @@ void HDDisplay::stop()
*/
void HDDisplay::setActiveUAS(UASInterface* uas)
{
qDebug() << "ATTEMPTING TO SET UAS";
//qDebug() << "ATTEMPTING TO SET UAS";
if (this->uas != NULL && this->uas != uas)
{
// Disconnect any previously connected active MAV
......@@ -144,7 +172,7 @@ void HDDisplay::setActiveUAS(UASInterface* uas)
//if (this->uas != uas)
// {
qDebug() << "UAS SET!" << "ID:" << uas->getUASID();
//qDebug() << "UAS SET!" << "ID:" << uas->getUASID();
// Setup communication
connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
//}
......@@ -349,11 +377,8 @@ void HDDisplay::drawSystemIndicator(float xRef, float yRef, int maxNum, float ma
float height = 1.5f;
const float hspacing = 0.6f;
// TODO ensure that instrument stays smaller than maxWidth and maxHeight
int i = 0;
while (value.hasNext() && i < maxNum)
while (value.hasNext() && i < maxNum && x < maxWidth && y < maxHeight)
{
value.next();
QBrush brush(Qt::SolidPattern);
......
......@@ -36,6 +36,8 @@ This file is part of the PIXHAWK project
#include <QColor>
#include <QTimer>
#include <QFontDatabase>
#include <QMap>
#include <QPair>
#include <cmath>
#include "UASInterface.h"
......@@ -47,7 +49,7 @@ namespace Ui {
class HDDisplay : public QWidget {
Q_OBJECT
public:
HDDisplay(QWidget *parent = 0);
HDDisplay(QStringList* plotList, QWidget *parent = 0);
~HDDisplay();
public slots:
......@@ -84,6 +86,10 @@ protected:
QMap<QString, float> valuesMean; ///< Mean since system startup for this variable
QMap<QString, int> valuesCount; ///< Number of values received so far
QMap<QString, quint64> lastUpdate; ///< The last update time for this variable
QMap<QString, float> minValues; ///< The minimum value this variable is assumed to have
QMap<QString, float> maxValues; ///< The maximum value this variable is assumed to have
QMap<QString, QPair<float, float> > goodRanges; ///< The range of good values
QMap<QString, QPair<float, float> > critRanges; ///< The range of critical values
double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates
float xCenterOffset, yCenterOffset; ///< Offset from center of window in mm coordinates
float vwidth; ///< Virtual width of this window, 200 mm per default. This allows to hardcode positions and aspect ratios. This virtual image plane is then scaled to the window size.
......@@ -114,6 +120,8 @@ protected:
float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD
float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD
QStringList* acceptList; ///< Variable names to plot
private:
Ui::HDDisplay *m_ui;
};
......
......@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project
#include <QDebug>
#include <cmath>
#include <limits>
#include "UASManager.h"
#include "HUD.h"
......@@ -41,6 +42,20 @@ This file is part of the PIXHAWK project
#define GL_MULTISAMPLE 0x809D
#endif
template<typename T>
inline bool isnan(T value)
{
return value != value;
}
// requires #include <limits>
template<typename T>
inline bool isinf(T value)
{
return std::numeric_limits<T>::has_infinity && (value == std::numeric_limits<T>::infinity() || (-1*value) == std::numeric_limits<T>::infinity());
}
/**
* @warning The HUD widget will not start painting its content automatically
* to update the view, start the auto-update by calling HUD::start().
......@@ -158,16 +173,25 @@ void HUD::updateValue(UASInterface* uas, QString name, double value, quint64 mse
{
// if (this->uas == uas)
//{
if (!isnan(value) && !isinf(value))
{
// Update mean
const float oldMean = valuesMean.value(name, 0.0f);
const int meanCount = valuesCount.value(name, 0);
valuesMean.insert(name, (oldMean * meanCount + value) / (meanCount + 1));
double mean = (oldMean * meanCount + value) / (meanCount + 1);
if (isnan(mean) || isinf(mean)) mean = 0.0;
valuesMean.insert(name, mean);
valuesCount.insert(name, meanCount + 1);
// Two-value sliding average
valuesDot.insert(name, (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f);
double dot = (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f;
if (isnan(dot) || isinf(dot)) dot = 0.0;
valuesDot.insert(name, dot);
values.insert(name, value);
lastUpdate.insert(name, msec);
//}
qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount;
}
}
/**
......@@ -477,9 +501,9 @@ void HUD::paintRollPitchStrips()
void HUD::paintGL()
{
// Read out most important values to limit hash table lookups
const float roll = values.value("roll", 0.0f);
const float pitch = values.value("pitch", 0.0f);
const float yaw = values.value("yaw", 0.0f);
static float roll = roll * 0.5 + 0.5 * values.value("roll", 0.0f);
static float pitch = pitch * 0.5 + 0.5 * values.value("pitch", 0.0f);
static float yaw = yaw * 0.5 + 0.5 * values.value("yaw", 0.0f);
//qDebug() << __FILE__ << __LINE__ << "ROLL:" << roll << "PITCH:" << pitch << "YAW:" << yaw;
......@@ -616,20 +640,15 @@ void HUD::paintGL()
// MOVING PARTS
painter.translate(0, (pitch/M_PI)* -180.0f * refToScreenY(2.0f));
// Translate for yaw
const float maxYawTrans = 100.0f;
const float maxYawTrans = 60.0f;
float yawDiff = valuesDot.value("yaw", 0.0f);
while (yawDiff > M_PI)
{
yawDiff = yawDiff - M_PI;
}
if (isinf(yawDiff)) yawDiff = 0.0f;
if (yawDiff > M_PI) yawDiff = yawDiff - M_PI;
while (yawDiff < -M_PI)
{
yawDiff = yawDiff + M_PI;
}