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#colorIcon {}
QWidget { QWidget {
background-color: #151518; background-color: #050508;
color: #DDDDDF; color: #DDDDDF;
background-clip: border; background-clip: border;
font-size: 11px; font-size: 11px;
} }
QGroupBox { QGroupBox {
border: 1px solid #777777; border: 1px solid #66666B;
border-radius: 3px; border-radius: 3px;
padding: 10px 0px 0px 0px; padding: 10px 0px 0px 0px;
margin-top: 1ex; /* leave space at the top for the title */ margin-top: 1ex; /* leave space at the top for the title */
...@@ -107,11 +107,11 @@ QPushButton { ...@@ -107,11 +107,11 @@ QPushButton {
font-weight: bold; font-weight: bold;
min-height: 18px; min-height: 18px;
max-height: 32px; max-height: 32px;
border: 1px solid #EEEEEE; border: 2px solid #4A4A4F;
border-radius: 5px; border-radius: 5px;
padding-left: 10px; padding-left: 10px;
padding-right: 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 { QPushButton:checked {
...@@ -186,7 +186,7 @@ QPushButton:pressed#killButton { ...@@ -186,7 +186,7 @@ QPushButton:pressed#killButton {
QPushButton#controlButton { QPushButton#controlButton {
min-height: 25px; 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 { QPushButton:checked#controlButton {
...@@ -194,12 +194,12 @@ QPushButton:checked#controlButton { ...@@ -194,12 +194,12 @@ QPushButton:checked#controlButton {
} }
QProgressBar { QProgressBar {
border: 1px solid white; border: 1px solid #EEEEEE;
border-radius: 4px; border-radius: 4px;
text-align: center; text-align: center;
padding: 2px; padding: 2px;
color: white; color: white;
background-color: #111111; background-color: #111118;
} }
QProgressBar:horizontal { QProgressBar:horizontal {
......
...@@ -78,7 +78,7 @@ HEADERS += src/MG.h \ ...@@ -78,7 +78,7 @@ HEADERS += src/MG.h \
src/configuration.h \ src/configuration.h \
src/ui/uas/UASView.h \ src/ui/uas/UASView.h \
src/ui/CameraView.h \ src/ui/CameraView.h \
src/comm/MavlinkSimulationLink.h \ src/comm/MAVLinkSimulationLink.h \
src/comm/UDPLink.h \ src/comm/UDPLink.h \
src/ui/ParameterInterface.h \ src/ui/ParameterInterface.h \
src/ui/WaypointList.h \ src/ui/WaypointList.h \
...@@ -129,7 +129,7 @@ SOURCES += src/main.cc \ ...@@ -129,7 +129,7 @@ SOURCES += src/main.cc \
src/ui/linechart/ScrollZoomer.cc \ src/ui/linechart/ScrollZoomer.cc \
src/ui/uas/UASView.cc \ src/ui/uas/UASView.cc \
src/ui/CameraView.cc \ src/ui/CameraView.cc \
src/comm/MavlinkSimulationLink.cc \ src/comm/MAVLinkSimulationLink.cc \
src/comm/UDPLink.cc \ src/comm/UDPLink.cc \
src/ui/ParameterInterface.cc \ src/ui/ParameterInterface.cc \
src/ui/WaypointList.cc \ src/ui/WaypointList.cc \
......
/*===================================================================== /*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
(c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch> (c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
This file is part of the PIXHAWK project This file is part of the PIXHAWK project
PIXHAWK is free software: you can redistribute it and/or modify PIXHAWK is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
PIXHAWK is distributed in the hope that it will be useful, PIXHAWK is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>. along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/ ======================================================================*/
/** /**
* @file * @file
* @brief Implementation of simulated system link * @brief Implementation of simulated system link
...@@ -53,17 +53,18 @@ This file is part of the PIXHAWK project ...@@ -53,17 +53,18 @@ This file is part of the PIXHAWK project
* @param writeFile The received messages are written to that file * @param writeFile The received messages are written to that file
* @param rate The rate at which the messages are sent (in intervals of milliseconds) * @param rate The rate at which the messages are sent (in intervals of milliseconds)
**/ **/
MAVLinkSimulationLink::MAVLinkSimulationLink(QFile* readFile, QFile* writeFile, int rate) : MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile, int rate) :
readyBytes(0) readyBytes(0),
timeOffset(0)
{ {
this->id = getNextLinkId(); this->id = getNextLinkId();
LinkManager::instance()->add(this); LinkManager::instance()->add(this);
this->rate = rate; this->rate = rate;
_isConnected = false; _isConnected = false;
if (readFile != NULL) if (readFile != "")
{ {
this->name = "Simulation: " + readFile->fileName(); this->name = "Simulation: " + readFile;
} }
else else
{ {
...@@ -72,20 +73,18 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QFile* readFile, QFile* writeFile, ...@@ -72,20 +73,18 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QFile* readFile, QFile* writeFile,
// Comments on the variables can be found in the header file // Comments on the variables can be found in the header file
simulationFile = readFile; simulationFile = new QFile(readFile, this);
simulationHeader = readFile->readLine(); if (simulationFile->exists() && simulationFile->open(QIODevice::ReadOnly | QIODevice::Text))
receiveFile = writeFile; {
simulationHeader = simulationFile->readLine();
}
receiveFile = new QFile(writeFile, this);
lastSent = MG::TIME::getGroundTimeNow(); lastSent = MG::TIME::getGroundTimeNow();
// Initialize the pseudo-random number generator // Initialize the pseudo-random number generator
srand(QTime::currentTime().msec()); srand(QTime::currentTime().msec());
maxTimeNoise = 0; maxTimeNoise = 0;
// timer = new QTimer(this);
// QObject::connect(timer, SIGNAL(timeout()), this, SLOT(mainloop()));
// _isConnected = false;
// this->rate = rate;
} }
MAVLinkSimulationLink::~MAVLinkSimulationLink() MAVLinkSimulationLink::~MAVLinkSimulationLink()
...@@ -97,6 +96,13 @@ MAVLinkSimulationLink::~MAVLinkSimulationLink() ...@@ -97,6 +96,13 @@ MAVLinkSimulationLink::~MAVLinkSimulationLink()
void MAVLinkSimulationLink::run() void MAVLinkSimulationLink::run()
{ {
status.mode = MAV_MODE_UNINIT;
status.status = MAV_STATE_UNINIT;
status.vbat = 0;
status.motor_block = 1;
status.packet_drop = 0;
forever forever
{ {
...@@ -116,72 +122,41 @@ void MAVLinkSimulationLink::run() ...@@ -116,72 +122,41 @@ void MAVLinkSimulationLink::run()
} }
} }
void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
{
// Allocate buffer with packet data
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
unsigned int bufferlength = message_to_send_buffer(buf, msg);
//add data into datastream
memcpy(stream+(*index),buf, bufferlength);
(*index) += bufferlength;
}
void MAVLinkSimulationLink::mainloop() void MAVLinkSimulationLink::mainloop()
{ {
// Test for encoding / decoding packets // Test for encoding / decoding packets
// Test data stream // Test data stream
const int streamlength = 1024; const int streamlength = 4096;
int streampointer = 0; int streampointer = 0;
//const int testoffset = 0; //const int testoffset = 0;
uint8_t stream[streamlength]; uint8_t stream[streamlength] = {};
// Fake system values // Fake system values
uint8_t systemId = 220; uint8_t systemId = 220;
uint8_t componentId = 0; uint8_t componentId = 0;
uint16_t version = 1000; uint16_t version = 1000;
// Fake sensor values
uint32_t xacc = 0;
uint32_t yacc = 0;
uint32_t zacc = 9810;
uint32_t xgyro = 10;
uint32_t ygyro = 5;
uint32_t zgyro = 100;
uint32_t xmag = 0;
uint32_t ymag = 1000;
uint32_t zmag = 500;
uint32_t pressure = 20000;
uint32_t grounddist = 500;
uint32_t temp = 20000;
static float fullVoltage = 4.2 * 3; static float fullVoltage = 4.2 * 3;
static float emptyVoltage = 3.35 * 3; static float emptyVoltage = 3.35 * 3;
static float voltage = fullVoltage; static float voltage = fullVoltage;
static float drainRate = 0.0025; // x.xx% of the capacity is linearly drained per second static float drainRate = 0.0025; // x.xx% of the capacity is linearly drained per second
// uint32_t chan1 = 1500;
// uint32_t chan2 = 1500;
// uint32_t chan3 = 1500;
// uint32_t chan4 = 1500;
// uint32_t chan5 = 1500;
float act1 = 0.1;
float act2 = 0.2;
float act3 = 0.3;
float act4 = 0.4;
// Fake coordinates
float roll = 0.0f;
float pitch = 10.0f;
float yaw = 60.5f;
// Fake Marker positions attitude_t attitude;
raw_aux_t rawAuxValues;
uint32_t markerId = 20; raw_imu_t rawImuValues;
float confidence = 100.0f; raw_sensor_t rawSensorValues;
float posX = -1.0f;
float posY = 1.0f;
float posZ = 2.5f;
// Fake timestamp
unsigned int usec = MG::TIME::getGroundTimeNow();
uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
int bufferlength; int bufferlength;
...@@ -200,80 +175,238 @@ void MAVLinkSimulationLink::mainloop() ...@@ -200,80 +175,238 @@ void MAVLinkSimulationLink::mainloop()
voltage = voltage - ((fullVoltage - emptyVoltage) * drainRate / rate); voltage = voltage - ((fullVoltage - emptyVoltage) * drainRate / rate);
if (voltage < 3.550 * 3) voltage = 3.550 * 3; if (voltage < 3.550 * 3) voltage = 3.550 * 3;
//uint8_t msgbuffer[COMM_MAX_PACKET_LEN]; static int state = 0;
// 100 HZ TASKS if (state == 0)
if (rate50hzCounter == 1000 / rate / 100)
{ {
// Read values // BOOT
char buf[1024]; // Pack message and get size of encoded byte string
qint64 lineLength = simulationFile->readLine(buf, sizeof(buf)); messageSize = message_boot_pack(systemId, componentId, &msg, version);
if (lineLength != -1) // Allocate buffer with packet data
{ bufferlength = message_to_send_buffer(buffer, &msg);
// Data is available //add data into datastream
} memcpy(stream+streampointer,buffer, bufferlength);
else streampointer += bufferlength;
state++;
}
// 50 HZ TASKS
if (rate50hzCounter == 1000 / rate / 40)
{
if (simulationFile->isOpen())
{ {
// We reached the end of the file, start from scratch if (simulationFile->atEnd())
file.reset(); {
//simulationHeader // We reached the end of the file, start from scratch
simulationFile->reset();
simulationHeader = simulationFile->readLine();
}
// Data was made available, read one line
// first entry is the timestamp
QString values = QString(simulationFile->readLine());
QStringList parts = values.split("\t");
QStringList keys = simulationHeader.split("\t");
//qDebug() << simulationHeader;
//qDebug() << values;
bool ok;
static quint64 lastTime = 0;
static quint64 baseTime = 0;
quint64 time = QString(parts.first()).toLongLong(&ok, 10);
if (ok)
{
if (timeOffset == 0)
{
timeOffset = time;
baseTime = time;
}
if (lastTime > time)
{
// We have wrapped around in the logfile
// Add the measurement time interval to the base time
baseTime += lastTime - timeOffset;
}
lastTime = time;
time = time - timeOffset + baseTime;
// Gather individual measurement values
for (int i = 1; i < (parts.size() - 1); ++i)
{
// Get one data field
bool res;
double d = QString(parts.at(i)).toDouble(&res);
if (!res) d = 0;
//qDebug() << "TIME" << time << "VALUE" << d;
//emit valueChanged(220, keys.at(i), d, MG::TIME::getGroundTimeNow());
if (keys.value(i, "") == "Accel._X")
{
rawImuValues.xacc = d;
}
if (keys.value(i, "") == "Accel._Y")
{
rawImuValues.yacc = d;
}
if (keys.value(i, "") == "Accel._Z")
{
rawImuValues.zacc = d;
}
if (keys.value(i, "") == "Gyro_Phi")
{
rawImuValues.xgyro = d;
}
if (keys.value(i, "") == "Gyro_Theta")
{
rawImuValues.ygyro = d;
}
if (keys.value(i, "") == "Gyro_Psi")
{
rawImuValues.zgyro = d;
}
if (keys.value(i, "") == "Pressure")
{
rawAuxValues.baro = d;
}
if (keys.value(i, "") == "Battery")
{
rawAuxValues.vbat = d;
}
if (keys.value(i, "") == "roll_IMU")
{
attitude.roll = d;
}
if (keys.value(i, "") == "pitch_IMU")
{
attitude.pitch = d;
}
if (keys.value(i, "") == "yaw_IMU")
{
attitude.yaw = d;
}
//Accel._X Accel._Y Accel._Z Battery Bottom_Rotor CPU_Load Ground_Dist. Gyro_Phi Gyro_Psi Gyro_Theta Left_Servo Mag._X Mag._Y Mag._Z Pressure Right_Servo Temperature Top_Rotor pitch_IMU roll_IMU yaw_IMU
}
// Send out packets
// ATTITUDE
attitude.msec = time;
// Pack message and get size of encoded byte string
message_attitude_encode(systemId, componentId, &msg, &attitude);
// Allocate buffer with packet data
bufferlength = message_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
// IMU
rawImuValues.msec = time;
rawImuValues.xmag = 0;
rawImuValues.ymag = 0;
rawImuValues.zmag = 0;
// Pack message and get size of encoded byte string
message_raw_imu_encode(systemId, componentId, &msg, &rawImuValues);
// Allocate buffer with packet data
bufferlength = message_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
//qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer;
//qDebug() << "REALTIME" << MG::TIME::getGroundTimeNow() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
}
} }
rate50hzCounter = 1;
}
// 10 HZ TASKS
if (rate10hzCounter == 1000 / rate / 9)
{
rate10hzCounter = 1;
} }
// 1 HZ TASKS // 1 HZ TASKS
if (rate1hzCounter == 1000 / rate / 1) if (rate1hzCounter == 1000 / rate / 1)
{ {
// BOOT // STATE
static int statusCounter = 0;
if (statusCounter == 100)
{
status.mode = (status.mode + 1) % MAV_MODE_TEST3;
statusCounter = 0;
}
statusCounter++;
status.vbat = voltage;
// Pack message and get size of encoded byte string // Pack message and get size of encoded byte string
messageSize = message_boot_pack(systemId, componentId, &msg, version); messageSize = message_sys_status_encode(systemId, componentId, &msg, &status);
// Allocate buffer with packet data // Allocate buffer with packet data
bufferlength = message_to_send_buffer(buffer, &msg); bufferlength = message_to_send_buffer(buffer, &msg);
//add data into datastream //add data into datastream
memcpy(stream+streampointer,buffer, bufferlength); memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength; streampointer += bufferlength;
/*
// Pack message and get size of encoded byte string
messageSize = message_boot_pack(systemId, componentId, &msg, version);
// Allocate buffer with packet data
bufferlength = message_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;*/
// HEARTBEAT // HEARTBEAT
static int typeCounter = 0;
uint8_t mavType = typeCounter % (OCU);
typeCounter++;
// Pack message and get size of encoded byte string // Pack message and get size of encoded byte string
messageSize = message_heartbeat_pack(systemId, componentId, &msg, MAV_GENERIC); messageSize = message_heartbeat_pack(systemId, componentId, &msg, mavType);
// Allocate buffer with packet data // Allocate buffer with packet data
bufferlength = message_to_send_buffer(buffer, &msg); bufferlength = message_to_send_buffer(buffer, &msg);
//add data into datastream //add data into datastream
memcpy(stream+streampointer,buffer, bufferlength); memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength; streampointer += bufferlength;
readyBufferMutex.lock(); //qDebug() << "BOOT" << "BUF LEN" << bufferlength << "POINTER" << streampointer;
for (int i = 0; i < streampointer; i++)
{
readyBuffer.enqueue(*(stream + i));
}
readyBufferMutex.unlock();
rate1hzCounter = 1;
}
// 10 HZ TASKS // AUX STATUS
if (rate10hzCounter == 1000 / rate / 10) rawAuxValues.vbat = voltage;
{
readyBufferMutex.lock(); rate1hzCounter = 1;
for (int i = 0; i < streampointer; i++)
{
readyBuffer.enqueue(*(stream + i));
}
readyBufferMutex.unlock();
rate10hzCounter = 1;
} }
// FULL RATE TASKS // FULL RATE TASKS
// Default is 50 Hz // Default is 50 Hz
/*
// 50 HZ TASKS // 50 HZ TASKS
if (rate50hzCounter == 1000 / rate / 50) if (rate50hzCounter == 1000 / rate / 50)
{ {
streampointer = 0; //streampointer = 0;
// Attitude // Attitude
...@@ -286,7 +419,7 @@ void MAVLinkSimulationLink::mainloop() ...@@ -286,7 +419,7 @@ void MAVLinkSimulationLink::mainloop()
streampointer += bufferlength; streampointer += bufferlength;
rate50hzCounter = 1; rate50hzCounter = 1;
} }*/
readyBufferMutex.lock(); readyBufferMutex.lock();
for (int i = 0; i < streampointer; i++) for (int i = 0; i < streampointer; i++)
...@@ -317,14 +450,69 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) ...@@ -317,14 +450,69 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
// Increase write counter // Increase write counter
//bitsSentTotal += size * 8; //bitsSentTotal += size * 8;
// Parse bytes
mavlink_message_t msg;
mavlink_status_t comm;
// Output all bytes as hex digits // Output all bytes as hex digits
int i; int i;
for (i=0; i<size; i++) for (i=0; i<size; i++)
{ {
if (mavlink_parse_char(this->id, data[i], &msg, &comm))
{
// MESSAGE RECEIVED!
switch (msg.msgid)
{
// SET THE SYSTEM MODE
case MAVLINK_MSG_ID_SET_MODE:
{
set_mode_t mode;
message_set_mode_decode(&msg, &mode);
// Set mode indepent of mode.target
status.mode = mode.mode;
}
// EXECUTE OPERATOR ACTIONS
case MAVLINK_MSG_ID_ACTION:
{
action_t action;
message_action_decode(&msg, &action);
switch (action.action)
{
case MAV_ACTION_LAUNCH:
status.status = MAV_STATE_ACTIVE;
status.mode = MAV_MODE_AUTO;
break;
case MAV_ACTION_RETURN:
break;
case MAV_ACTION_MOTORS_START:
status.status = MAV_STATE_ACTIVE;
status.mode = MAV_MODE_LOCKED;
break;
case MAV_ACTION_MOTORS_STOP:
status.status = MAV_STATE_STANDBY;
status.mode = MAV_MODE_LOCKED;
break;
case MAV_ACTION_EMCY_KILL:
status.status = MAV_STATE_EMERGENCY;
status.mode = MAV_MODE_MANUAL;
break;
}
}
break;
}
}
unsigned char v=data[i]; unsigned char v=data[i];
fprintf(stderr,"%02x ", v); fprintf(stderr,"%02x ", v);
} }
fprintf(stderr,"\n"); fprintf(stderr,"\n");
// Update comm status
status.packet_drop = comm.packet_rx_drop_count;
} }
......
...@@ -38,12 +38,14 @@ This file is part of the PIXHAWK project ...@@ -38,12 +38,14 @@ This file is part of the PIXHAWK project
#include <QQueue> #include <QQueue>
#include <QMutex> #include <QMutex>
#include <inttypes.h> #include <inttypes.h>
#include <LinkInterface.h> #include <mavlink.h>
#include "LinkInterface.h"
class MAVLinkSimulationLink : public LinkInterface class MAVLinkSimulationLink : public LinkInterface
{ {
Q_OBJECT
public: public:
MAVLinkSimulationLink(QFile* readFile=NULL, QFile* writeFile=NULL, int rate=5); MAVLinkSimulationLink(QString readFile="", QString writeFile="", int rate=5);
~MAVLinkSimulationLink(); ~MAVLinkSimulationLink();
bool isConnected(); bool isConnected();
qint64 bytesAvailable(); qint64 bytesAvailable();
...@@ -94,7 +96,6 @@ protected: ...@@ -94,7 +96,6 @@ protected:
/** File which contains the input data (simulated robot messages) **/ /** File which contains the input data (simulated robot messages) **/
QFile* simulationFile; QFile* simulationFile;
QString simulationHeader; QString simulationHeader;
int lineCounter = 0;
/** File where the commands sent by the groundstation are stored **/ /** File where the commands sent by the groundstation are stored **/
QFile* receiveFile; QFile* receiveFile;
QTextStream stream; QTextStream stream;
...@@ -112,9 +113,15 @@ protected: ...@@ -112,9 +113,15 @@ protected:
int id; int id;
QString name; QString name;
qint64 timeOffset;
sys_status_t status;
void setMaximumTimeNoise(int milliseconds); void setMaximumTimeNoise(int milliseconds);
void addTimeNoise(); 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) ...@@ -113,6 +113,50 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit statusChanged(this, uasState, stateDescription); emit statusChanged(this, uasState, stateDescription);
onboardTimeOffset = 0; // Reset offset measurement onboardTimeOffset = 0; // Reset offset measurement
break; 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: case MAVLINK_MSG_ID_SYSTEM:
// std::cerr << std::endl; // std::cerr << std::endl;
...@@ -198,9 +242,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -198,9 +242,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
//std::cerr << "BYTES RESSOURCE RECEIVED" << std::endl; //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)); 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; 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) if (time == 0)
{ {
time = MG::TIME::getGroundTimeNow(); time = MG::TIME::getGroundTimeNow();
...@@ -214,21 +261,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -214,21 +261,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
time += onboardTimeOffset; time += onboardTimeOffset;
} }
emit valueChanged(uasId, "Accel. X", message_sensraw_get_xacc(message.payload), time); emit valueChanged(uasId, "Accel. X", raw.xacc, time);
emit valueChanged(uasId, "Accel. Y", message_sensraw_get_yacc(message.payload), time); emit valueChanged(uasId, "Accel. Y", raw.yacc, time);
emit valueChanged(uasId, "Accel. Z", message_sensraw_get_zacc(message.payload), time); emit valueChanged(uasId, "Accel. Z", raw.zacc, time);
emit valueChanged(uasId, "Gyro Phi", message_sensraw_get_xgyro(message.payload), time); emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time);
emit valueChanged(uasId, "Gyro Theta", message_sensraw_get_ygyro(message.payload), time); emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time);
emit valueChanged(uasId, "Gyro Psi", message_sensraw_get_zgyro(message.payload), time); emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time);
emit valueChanged(uasId, "Mag. X", message_sensraw_get_xmag(message.payload), time); emit valueChanged(uasId, "Mag. X", raw.xmag, time);
emit valueChanged(uasId, "Mag. Y", message_sensraw_get_ymag(message.payload), time); emit valueChanged(uasId, "Mag. Y", raw.ymag, time);
emit valueChanged(uasId, "Mag. Z", message_sensraw_get_zmag(message.payload), time); emit valueChanged(uasId, "Mag. Z", raw.zmag, 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);
} }
break; break;
*/
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
//std::cerr << std::endl; //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; //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) ...@@ -338,12 +381,15 @@ void UAS::setAutoMode(bool autoMode)
} }
} }
void UAS::setMode(enum MAV_MODE mode) void UAS::setMode(int mode)
{ {
this->mode = mode; if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
mavlink_message_t msg; {
message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode); this->mode = mode;
sendMessage(msg); 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) void UAS::sendMessage(mavlink_message_t message)
......
...@@ -115,7 +115,7 @@ protected: ...@@ -115,7 +115,7 @@ protected:
bool controlPitchManual; ///< status flag, true if pitch is controlled manually bool controlPitchManual; ///< status flag, true if pitch is controlled manually
bool controlYawManual; ///< status flag, true if yaw is controlled manually bool controlYawManual; ///< status flag, true if yaw is controlled manually
bool controlThrustManual;///< status flag, true if thrust 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; quint64 onboardTimeOffset;
/** @brief Set the current battery type */ /** @brief Set the current battery type */
...@@ -178,7 +178,7 @@ public slots: ...@@ -178,7 +178,7 @@ public slots:
void setSelected(); void setSelected();
/** @brief Set current mode of operation, e.g. auto or manual */ /** @brief Set current mode of operation, e.g. auto or manual */
void setMode(enum MAV_MODE mode); void setMode(int mode);
signals: signals:
......
/*===================================================================== /*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit 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> (c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
...@@ -159,6 +158,8 @@ public slots: ...@@ -159,6 +158,8 @@ public slots:
* @param autoMode true for autonomous operation, false for manual control * @param autoMode true for autonomous operation, false for manual control
*/ */
virtual void setAutoMode(bool autoMode) = 0; 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 **/ /** Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/
virtual void emergencySTOP() = 0; 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 **/ /** 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: ...@@ -210,6 +211,8 @@ signals:
* @param description longer textual description. Should be however limited to a short text, e.g. 200 chars. * @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); 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 **/ /** @brief A command has been issued **/
void commandSent(int command); void commandSent(int command);
/** @brief The connection status has changed **/ /** @brief The connection status has changed **/
......
...@@ -31,13 +31,14 @@ This file is part of the PIXHAWK project ...@@ -31,13 +31,14 @@ This file is part of the PIXHAWK project
#include <QFile> #include <QFile>
#include <QGLWidget> #include <QGLWidget>
#include <QStringList>
#include "UASManager.h" #include "UASManager.h"
#include "HDDisplay.h" #include "HDDisplay.h"
#include "ui_HDDisplay.h" #include "ui_HDDisplay.h"
#include <QDebug> #include <QDebug>
HDDisplay::HDDisplay(QWidget *parent) : HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) :
QWidget(parent), QWidget(parent),
uas(NULL), uas(NULL),
values(QMap<QString, float>()), values(QMap<QString, float>()),
...@@ -45,6 +46,10 @@ HDDisplay::HDDisplay(QWidget *parent) : ...@@ -45,6 +46,10 @@ HDDisplay::HDDisplay(QWidget *parent) :
valuesMean(QMap<QString, float>()), valuesMean(QMap<QString, float>()),
valuesCount(QMap<QString, int>()), valuesCount(QMap<QString, int>()),
lastUpdate(QMap<QString, quint64>()), lastUpdate(QMap<QString, quint64>()),
minValues(),
maxValues(),
goodRanges(),
critRanges(),
xCenterOffset(0.0f), xCenterOffset(0.0f),
yCenterOffset(0.0f), yCenterOffset(0.0f),
vwidth(80.0f), vwidth(80.0f),
...@@ -62,6 +67,7 @@ HDDisplay::HDDisplay(QWidget *parent) : ...@@ -62,6 +67,7 @@ HDDisplay::HDDisplay(QWidget *parent) :
strongStrokeWidth(1.5f), strongStrokeWidth(1.5f),
normalStrokeWidth(1.0f), normalStrokeWidth(1.0f),
fineStrokeWidth(0.5f), fineStrokeWidth(0.5f),
acceptList(plotList),
m_ui(new Ui::HDDisplay) m_ui(new Ui::HDDisplay)
{ {
//m_ui->setupUi(this); //m_ui->setupUi(this);
...@@ -82,7 +88,7 @@ HDDisplay::HDDisplay(QWidget *parent) : ...@@ -82,7 +88,7 @@ HDDisplay::HDDisplay(QWidget *parent) :
if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName; if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName;
// Connect with UAS // Connect with UAS
//connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
//start(); //start();
} }
...@@ -93,7 +99,6 @@ HDDisplay::~HDDisplay() ...@@ -93,7 +99,6 @@ HDDisplay::~HDDisplay()
void HDDisplay::paintEvent(QPaintEvent * event) void HDDisplay::paintEvent(QPaintEvent * event)
{ {
paintGL(); paintGL();
} }
...@@ -108,13 +113,36 @@ void HDDisplay::paintGL() ...@@ -108,13 +113,36 @@ void HDDisplay::paintGL()
if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH; if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH;
QPainter painter(this); QPainter painter(this);
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
painter.fillRect(QRect(0, 0, width(), height()), backgroundColor); painter.fillRect(QRect(0, 0, width(), height()), backgroundColor);
painter.setRenderHint(QPainter::HighQualityAntialiasing); const int columns = 3;
const float gaugeWidth = 15.0f; 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); const QColor gaugeColor = QColor(200, 200, 200);
drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter); //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(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(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); //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() void HDDisplay::start()
...@@ -133,7 +161,7 @@ void HDDisplay::stop() ...@@ -133,7 +161,7 @@ void HDDisplay::stop()
*/ */
void HDDisplay::setActiveUAS(UASInterface* uas) void HDDisplay::setActiveUAS(UASInterface* uas)
{ {
qDebug() << "ATTEMPTING TO SET UAS"; //qDebug() << "ATTEMPTING TO SET UAS";
if (this->uas != NULL && this->uas != uas) if (this->uas != NULL && this->uas != uas)
{ {
// Disconnect any previously connected active MAV // Disconnect any previously connected active MAV
...@@ -144,7 +172,7 @@ void HDDisplay::setActiveUAS(UASInterface* uas) ...@@ -144,7 +172,7 @@ void HDDisplay::setActiveUAS(UASInterface* uas)
//if (this->uas != uas) //if (this->uas != uas)
// { // {
qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); //qDebug() << "UAS SET!" << "ID:" << uas->getUASID();
// Setup communication // Setup communication
connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); 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 ...@@ -349,11 +377,8 @@ void HDDisplay::drawSystemIndicator(float xRef, float yRef, int maxNum, float ma
float height = 1.5f; float height = 1.5f;
const float hspacing = 0.6f; const float hspacing = 0.6f;
// TODO ensure that instrument stays smaller than maxWidth and maxHeight
int i = 0; int i = 0;
while (value.hasNext() && i < maxNum) while (value.hasNext() && i < maxNum && x < maxWidth && y < maxHeight)
{ {
value.next(); value.next();
QBrush brush(Qt::SolidPattern); QBrush brush(Qt::SolidPattern);
......
...@@ -36,6 +36,8 @@ This file is part of the PIXHAWK project ...@@ -36,6 +36,8 @@ This file is part of the PIXHAWK project
#include <QColor> #include <QColor>
#include <QTimer> #include <QTimer>
#include <QFontDatabase> #include <QFontDatabase>
#include <QMap>
#include <QPair>
#include <cmath> #include <cmath>
#include "UASInterface.h" #include "UASInterface.h"
...@@ -47,7 +49,7 @@ namespace Ui { ...@@ -47,7 +49,7 @@ namespace Ui {
class HDDisplay : public QWidget { class HDDisplay : public QWidget {
Q_OBJECT Q_OBJECT
public: public:
HDDisplay(QWidget *parent = 0); HDDisplay(QStringList* plotList, QWidget *parent = 0);
~HDDisplay(); ~HDDisplay();
public slots: public slots:
...@@ -84,6 +86,10 @@ protected: ...@@ -84,6 +86,10 @@ protected:
QMap<QString, float> valuesMean; ///< Mean since system startup for this variable QMap<QString, float> valuesMean; ///< Mean since system startup for this variable
QMap<QString, int> valuesCount; ///< Number of values received so far QMap<QString, int> valuesCount; ///< Number of values received so far
QMap<QString, quint64> lastUpdate; ///< The last update time for this variable 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 double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates
float xCenterOffset, yCenterOffset; ///< Offset from center of window in mm 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. 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: ...@@ -114,6 +120,8 @@ protected:
float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD float normalStrokeWidth; ///< Normal line stroke width, used throughout the HUD
float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD float fineStrokeWidth; ///< Fine line stroke width, used throughout the HUD
QStringList* acceptList; ///< Variable names to plot
private: private:
Ui::HDDisplay *m_ui; Ui::HDDisplay *m_ui;
}; };
......
...@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project ...@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project
#include <QDebug> #include <QDebug>
#include <cmath> #include <cmath>
#include <limits>
#include "UASManager.h" #include "UASManager.h"
#include "HUD.h" #include "HUD.h"
...@@ -41,6 +42,20 @@ This file is part of the PIXHAWK project ...@@ -41,6 +42,20 @@ This file is part of the PIXHAWK project
#define GL_MULTISAMPLE 0x809D #define GL_MULTISAMPLE 0x809D
#endif #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 * @warning The HUD widget will not start painting its content automatically
* to update the view, start the auto-update by calling HUD::start(). * 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 ...@@ -158,16 +173,25 @@ void HUD::updateValue(UASInterface* uas, QString name, double value, quint64 mse
{ {
// if (this->uas == uas) // if (this->uas == uas)
//{ //{
if (!isnan(value) && !isinf(value))
{
// Update mean // Update mean
const float oldMean = valuesMean.value(name, 0.0f); const float oldMean = valuesMean.value(name, 0.0f);
const int meanCount = valuesCount.value(name, 0); 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); valuesCount.insert(name, meanCount + 1);
// Two-value sliding average // 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); values.insert(name, value);
lastUpdate.insert(name, msec); lastUpdate.insert(name, msec);
//} //}
qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount;
}
} }
/** /**
...@@ -477,9 +501,9 @@ void HUD::paintRollPitchStrips() ...@@ -477,9 +501,9 @@ void HUD::paintRollPitchStrips()
void HUD::paintGL() void HUD::paintGL()
{ {
// Read out most important values to limit hash table lookups // Read out most important values to limit hash table lookups
const float roll = values.value("roll", 0.0f); static float roll = roll * 0.5 + 0.5 * values.value("roll", 0.0f);
const float pitch = values.value("pitch", 0.0f); static float pitch = pitch * 0.5 + 0.5 * values.value("pitch", 0.0f);
const float yaw = values.value("yaw", 0.0f); static float yaw = yaw * 0.5 + 0.5 * values.value("yaw", 0.0f);
//qDebug() << __FILE__ << __LINE__ << "ROLL:" << roll << "PITCH:" << pitch << "YAW:" << yaw; //qDebug() << __FILE__ << __LINE__ << "ROLL:" << roll << "PITCH:" << pitch << "YAW:" << yaw;
...@@ -616,20 +640,15 @@ void HUD::paintGL() ...@@ -616,20 +640,15 @@ void HUD::paintGL()
// MOVING PARTS // MOVING PARTS
painter.translate(0, (pitch/M_PI)* -180.0f * refToScreenY(2.0f));
// Translate for yaw // Translate for yaw
const float maxYawTrans = 100.0f; const float maxYawTrans = 60.0f;
float yawDiff = valuesDot.value("yaw", 0.0f); float yawDiff = valuesDot.value("yaw", 0.0f);
while (yawDiff > M_PI) if (isinf(yawDiff)) yawDiff = 0.0f;
{ if (yawDiff > M_PI) yawDiff = yawDiff - M_PI;
yawDiff = yawDiff - M_PI;
}
while (yawDiff < -M_PI) if (yawDiff < -M_PI) yawDiff = yawDiff + M_PI;
{
yawDiff = yawDiff + M_PI;
}
yawInt += yawDiff; yawInt += yawDiff;
...@@ -642,6 +661,8 @@ void HUD::paintGL() ...@@ -642,6 +661,8 @@ void HUD::paintGL()
painter.translate(refToScreenX(yawTrans), 0); painter.translate(refToScreenX(yawTrans), 0);
painter.translate(0, (pitch/M_PI)* -180.0f * refToScreenY(2.0f));
// Rotate view and draw all roll-dependent indicators // Rotate view and draw all roll-dependent indicators
painter.rotate((roll/M_PI)* -180.0f); painter.rotate((roll/M_PI)* -180.0f);
......
...@@ -96,10 +96,19 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent) ...@@ -96,10 +96,19 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent)
protocol->setVisible(false); protocol->setVisible(false);
parameters = new ParameterInterface(this); parameters = new ParameterInterface(this);
parameters->setVisible(false); parameters->setVisible(false);
/*headDown1 = new HDDisplay(this);
QStringList* acceptList = new QStringList();
acceptList->append("roll IMU");
acceptList->append("pitch IMU");
acceptList->append("yaw IMU");
headDown1 = new HDDisplay(acceptList, this);
headDown1->setVisible(false); headDown1->setVisible(false);
headDown2 = new HDDisplay(this);
headDown2->setVisible(false);*/ QStringList* acceptList2 = new QStringList();
acceptList2->append("Battery");
acceptList2->append("Pressure");
headDown2 = new HDDisplay(acceptList2, this);
headDown2->setVisible(false);
centerStack->addWidget(map); centerStack->addWidget(map);
centerStack->addWidget(hud); centerStack->addWidget(hud);
setCentralWidget(centerStack); setCentralWidget(centerStack);
...@@ -107,7 +116,8 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent) ...@@ -107,7 +116,8 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent)
// Get IPs // Get IPs
QList<QHostAddress> hostAddresses = QNetworkInterface::allAddresses(); QList<QHostAddress> hostAddresses = QNetworkInterface::allAddresses();
QString windowname = qApp->applicationName() + qApp->applicationVersion(); QString windowname = qApp->applicationName() + " " + qApp->applicationVersion();
/*
windowname.append(" (" + QHostInfo::localHostName() + ": "); windowname.append(" (" + QHostInfo::localHostName() + ": ");
bool prevAddr = false; bool prevAddr = false;
for (int i = 0; i < hostAddresses.size(); i++) for (int i = 0; i < hostAddresses.size(); i++)
...@@ -122,6 +132,7 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent) ...@@ -122,6 +132,7 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent)
} }
windowname.append(")"); windowname.append(")");
*/
setWindowTitle(windowname); setWindowTitle(windowname);
#ifndef Q_WS_MAC #ifndef Q_WS_MAC
...@@ -155,7 +166,8 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent) ...@@ -155,7 +166,8 @@ MGMainWindow::MGMainWindow(QWidget *parent) : QMainWindow(parent)
ui.menuNetwork->addAction(commWidget->getAction()); ui.menuNetwork->addAction(commWidget->getAction());
udpLink->connect(); udpLink->connect();
simulationLink = new MAVLinkSimulationLink(); simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt");
connect(simulationLink, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64)));
LinkManager::instance()->addProtocol(simulationLink, mavlink); LinkManager::instance()->addProtocol(simulationLink, mavlink);
//CommConfigurationWindow* simulationWidget = new CommConfigurationWindow(simulationLink, mavlink, this); //CommConfigurationWindow* simulationWidget = new CommConfigurationWindow(simulationLink, mavlink, this);
//ui.menuNetwork->addAction(commWidget->getAction()); //ui.menuNetwork->addAction(commWidget->getAction());
...@@ -177,6 +189,40 @@ QStatusBar* MGMainWindow::createStatusBar() ...@@ -177,6 +189,40 @@ QStatusBar* MGMainWindow::createStatusBar()
return bar; return bar;
} }
void MGMainWindow::startVideoCapture()
{
QString format = "bmp";
QString initialPath = QDir::currentPath() + tr("/untitled.") + format;
QString screenFileName = QFileDialog::getSaveFileName(this, tr("Save As"),
initialPath,
tr("%1 Files (*.%2);;All Files (*)")
.arg(format.toUpper())
.arg(format));
delete videoTimer;
videoTimer = new QTimer(this);
videoTimer->setInterval(40);
connect(videoTimer, SIGNAL(timeout()), this, SLOT(saveScreen()));
}
void MGMainWindow::stopVideoCapture()
{
videoTimer->stop();
// TODO Convert raw images to PNG
}
void MGMainWindow::saveScreen()
{
QPixmap window = QPixmap::grabWindow(this->winId());
QString format = "bmp";
if (!screenFileName.isEmpty())
{
window.save(screenFileName, format.toAscii());
}
}
void MGMainWindow::reloadStylesheet() void MGMainWindow::reloadStylesheet()
{ {
// Load style sheet // Load style sheet
...@@ -313,18 +359,19 @@ void MGMainWindow::loadPilotView() ...@@ -313,18 +359,19 @@ void MGMainWindow::loadPilotView()
// HEAD UP DISPLAY // HEAD UP DISPLAY
centerStack->setCurrentWidget(hud); centerStack->setCurrentWidget(hud);
hud->start(); hud->start();
/*headDown1->start();
headDown2->start();
//connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), pfd, SLOT(setActiveUAS(UASInterface*))); //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), pfd, SLOT(setActiveUAS(UASInterface*)));
QDockWidget* container1 = new QDockWidget(tr("Main Instruments"), this); QDockWidget* container1 = new QDockWidget(tr("Primary Flight Display"), this);
container1->setWidget(headDown1); container1->setWidget(headDown1);
addDockWidget(Qt::RightDockWidgetArea, container1); addDockWidget(Qt::RightDockWidgetArea, container1);
QDockWidget* container2 = new QDockWidget(tr("Auxiliary Instruments"), this); QDockWidget* container2 = new QDockWidget(tr("Payload Status"), this);
container2->setWidget(headDown2); container2->setWidget(headDown2);
addDockWidget(Qt::RightDockWidgetArea, container2); addDockWidget(Qt::RightDockWidgetArea, container2);
*/
headDown1->start();
headDown2->start();
this->show(); this->show();
} }
......
...@@ -114,6 +114,9 @@ public slots: ...@@ -114,6 +114,9 @@ public slots:
void addLink(); void addLink();
void configure(); void configure();
void UASCreated(UASInterface* uas); void UASCreated(UASInterface* uas);
void startVideoCapture();
void stopVideoCapture();
void saveScreen();
/** @brief Load view for pilot */ /** @brief Load view for pilot */
void loadPilotView(); void loadPilotView();
...@@ -154,6 +157,8 @@ protected: ...@@ -154,6 +157,8 @@ protected:
LogCompressor* comp; LogCompressor* comp;
QString screenFileName;
QTimer* videoTimer;
private: private:
Ui::MGMainWindow ui; Ui::MGMainWindow ui;
......
...@@ -245,7 +245,7 @@ QProgressBar::chunk#thrustBar { ...@@ -245,7 +245,7 @@ QProgressBar::chunk#thrustBar {
</widget> </widget>
</item> </item>
<item row="0" column="6" colspan="2"> <item row="0" column="6" colspan="2">
<widget class="QLabel" name="lastStatusLabel"> <widget class="QLabel" name="modeLabel">
<property name="maximumSize"> <property name="maximumSize">
<size> <size>
<width>16777215</width> <width>16777215</width>
...@@ -288,19 +288,7 @@ QProgressBar::chunk#thrustBar { ...@@ -288,19 +288,7 @@ QProgressBar::chunk#thrustBar {
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="6" rowspan="2"> <item row="3" column="6" rowspan="2" colspan="2">
<widget class="QProgressBar" name="loadBar">
<property name="font">
<font>
<pointsize>8</pointsize>
</font>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item row="3" column="7" rowspan="2">
<widget class="QProgressBar" name="thrustBar"> <widget class="QProgressBar" name="thrustBar">
<property name="font"> <property name="font">
<font> <font>
...@@ -402,19 +390,6 @@ QProgressBar::chunk#thrustBar { ...@@ -402,19 +390,6 @@ QProgressBar::chunk#thrustBar {
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="5" rowspan="3">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>5</width>
<height>5</height>
</size>
</property>
</spacer>
</item>
<item row="7" column="3" colspan="2"> <item row="7" column="3" colspan="2">
<widget class="QLabel" name="waypointLabel"> <widget class="QLabel" name="waypointLabel">
<property name="font"> <property name="font">
...@@ -495,13 +470,13 @@ QProgressBar::chunk#thrustBar { ...@@ -495,13 +470,13 @@ QProgressBar::chunk#thrustBar {
</property> </property>
</widget> </widget>
</item> </item>
<item row="6" column="6" rowspan="2" colspan="2"> <item row="6" column="5" rowspan="2" colspan="3">
<layout class="QHBoxLayout" name="horizontalLayout"> <layout class="QHBoxLayout" name="horizontalLayout">
<property name="spacing"> <property name="spacing">
<number>4</number> <number>4</number>
</property> </property>
<property name="sizeConstraint"> <property name="sizeConstraint">
<enum>QLayout::SetDefaultConstraint</enum> <enum>QLayout::SetMinimumSize</enum>
</property> </property>
<item> <item>
<widget class="QPushButton" name="liftoffButton"> <widget class="QPushButton" name="liftoffButton">
......
...@@ -117,6 +117,9 @@ void UASControlWidget::setMode(int mode) ...@@ -117,6 +117,9 @@ void UASControlWidget::setMode(int mode)
case MAV_MODE_TEST3: case MAV_MODE_TEST3:
break; break;
} }
// Set mode on system
if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3) this->uas->setMode(mode);
} }
void UASControlWidget::cycleContextButton() void UASControlWidget::cycleContextButton()
......
...@@ -23,7 +23,7 @@ This file is part of the PIXHAWK project ...@@ -23,7 +23,7 @@ This file is part of the PIXHAWK project
/** /**
* @file * @file
* @brief Display the airstrip of one unmanned vehicle in the UAS list * @brief Implementation of one airstrip
* *
* @author Lorenz Meier <mavteam@student.ethz.ch> * @author Lorenz Meier <mavteam@student.ethz.ch>
* *
...@@ -43,6 +43,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : ...@@ -43,6 +43,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
timeRemaining(0), timeRemaining(0),
state("UNKNOWN"), state("UNKNOWN"),
stateDesc(tr("Unknown system state")), stateDesc(tr("Unknown system state")),
mode("MAV_MODE_UNKNOWN"),
thrust(0),
m_ui(new Ui::UASView) m_ui(new Ui::UASView)
{ {
this->uas = uas; this->uas = uas;
...@@ -58,6 +60,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : ...@@ -58,6 +60,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(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(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString))); connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString)));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double))); connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));
//connect(uas, SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool))); //connect(uas, SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool)));
connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int))); connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int)));
...@@ -104,6 +107,11 @@ UASView::~UASView() ...@@ -104,6 +107,11 @@ UASView::~UASView()
delete m_ui; delete m_ui;
} }
void UASView::updateMode(int sysId, QString status, QString description)
{
if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status);
}
void UASView::mouseDoubleClickEvent (QMouseEvent * event) void UASView::mouseDoubleClickEvent (QMouseEvent * event)
{ {
UASManager::instance()->setActiveUAS(uas); UASManager::instance()->setActiveUAS(uas);
...@@ -269,7 +277,8 @@ void UASView::refresh() ...@@ -269,7 +277,8 @@ void UASView::refresh()
// Battery // Battery
m_ui->batteryBar->setValue(static_cast<int>(this->chargeLevel)); m_ui->batteryBar->setValue(static_cast<int>(this->chargeLevel));
m_ui->loadBar->setValue(static_cast<int>(this->load)); //m_ui->loadBar->setValue(static_cast<int>(this->load));
m_ui->thrustBar->setValue(this->thrust);
if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME) if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME)
{ {
......
/*===================================================================== /*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
Please see our website at <http://pixhawk.ethz.ch>
(c) 2009 PIXHAWK PROJECT <http://pixhawk.ethz.ch> (c) 2009 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
...@@ -24,7 +23,7 @@ This file is part of the PIXHAWK project ...@@ -24,7 +23,7 @@ This file is part of the PIXHAWK project
/** /**
* @file * @file
* @brief Display the airstrip of one unmanned vehicle in the UAS list * @brief Definition of one airstrip
* *
* @author Lorenz Meier <mavteam@student.ethz.ch> * @author Lorenz Meier <mavteam@student.ethz.ch>
* *
...@@ -57,6 +56,8 @@ public slots: ...@@ -57,6 +56,8 @@ public slots:
void updateGlobalPosition(UASInterface*, double lon, double lat, double alt, quint64 usec); void updateGlobalPosition(UASInterface*, double lon, double lat, double alt, quint64 usec);
void updateSpeed(UASInterface*, double x, double y, double z, quint64 usec); void updateSpeed(UASInterface*, double x, double y, double z, quint64 usec);
void updateState(UASInterface*, QString uasState, QString stateDescription); void updateState(UASInterface*, QString uasState, QString stateDescription);
/** @brief Update the MAV mode */
void updateMode(int sysId, QString status, QString description);
void updateLoad(UASInterface* uas, double load); void updateLoad(UASInterface* uas, double load);
void receiveValue(int uasid, QString id, double value, quint64 time); void receiveValue(int uasid, QString id, double value, quint64 time);
void refresh(); void refresh();
...@@ -78,6 +79,8 @@ protected: ...@@ -78,6 +79,8 @@ protected:
double load; double load;
QString state; QString state;
QString stateDesc; QString stateDesc;
QString mode;
double thrust;
void mouseDoubleClickEvent (QMouseEvent * event); void mouseDoubleClickEvent (QMouseEvent * event);
/** @brief Mouse enters the widget */ /** @brief Mouse enters the widget */
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment