From 06a542bf0675810c57b2c3e2c957abab138a6d5e Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Wed, 9 Jun 2010 08:07:15 +0200
Subject: [PATCH 1/2] Added initialization and safety checks to remove clutter
from UAS view
---
src/uas/UAS.cc | 25 ++++++++++++++++++++++---
src/ui/uas/UASView.cc | 44 +++++++++++++++++++++++++++++++++++++++++---
src/ui/uas/UASView.h | 1 +
3 files changed, 64 insertions(+), 6 deletions(-)
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 1d026b4..2608af3 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -323,14 +323,33 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_gps_raw_t pos;
mavlink_msg_gps_raw_decode(&message, &pos);
+
+ // SANITY CHECK
+ // only accept values in a realistic range
// quint64 time = getUnixTime(pos.usec);
quint64 time = MG::TIME::getGroundTimeNow();
emit valueChanged(uasId, "lat", pos.lat, time);
emit valueChanged(uasId, "lon", pos.lon, time);
+ // Check for NaN
+ int alt = pos.alt;
+ if (alt != alt)
+ {
+ alt = 0;
+ emit textMessageReceived(uasId, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
+ }
emit valueChanged(uasId, "alt", pos.alt, time);
- emit valueChanged(uasId, "speed", pos.v, time);
- //qDebug() << "GOT GPS RAW";
- emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
+ // Smaller than threshold and not NaN
+ if (pos.v < 1000000 && pos.v == pos.v)
+ {
+ emit valueChanged(uasId, "speed", pos.v, time);
+ //qDebug() << "GOT GPS RAW";
+ emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
+ }
+ else
+ {
+ emit textMessageReceived(uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
+ }
+ emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
}
break;
case MAVLINK_MSG_ID_GPS_STATUS:
diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc
index 10dbd9a..defc81c 100644
--- a/src/ui/uas/UASView.cc
+++ b/src/ui/uas/UASView.cc
@@ -40,16 +40,26 @@ This file is part of the PIXHAWK project
UASView::UASView(UASInterface* uas, QWidget *parent) :
QWidget(parent),
+ startTime(0),
timeRemaining(0),
+ chargeLevel(0),
+ uas(uas),
+ load(0),
state("UNKNOWN"),
stateDesc(tr("Unknown system state")),
mode("MAV_MODE_UNKNOWN"),
thrust(0),
isActive(false),
+ x(0),
+ y(0),
+ z(0),
+ totalSpeed(0),
+ lat(0),
+ lon(0),
+ alt(0),
+ groundDistance(0),
m_ui(new Ui::UASView)
{
- this->uas = uas;
-
m_ui->setupUi(this);
// Setup communication
@@ -316,9 +326,37 @@ void UASView::refresh()
position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z);
m_ui->positionLabel->setText(position);
QString globalPosition;
- globalPosition = globalPosition.sprintf("%02.2f %02.2f %02.2f m", lon, lat, alt);
+ QString latIndicator;
+ if (lat > 0)
+ {
+ latIndicator = "N";
+ }
+ else
+ {
+ latIndicator = "S";
+ }
+ QString lonIndicator;
+ if (lon > 0)
+ {
+ lonIndicator = "E";
+ }
+ else
+ {
+ lonIndicator = "W";
+ }
+ globalPosition = globalPosition.sprintf("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
m_ui->gpsLabel->setText(globalPosition);
+ // Altitude
+ if (groundDistance == 0 && alt != 0)
+ {
+ m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt));
+ }
+ else
+ {
+ m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance));
+ }
+
// Speed
QString speed;
speed = speed.sprintf("%02.2f m/s", totalSpeed);
diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h
index bcd91c1..928423e 100644
--- a/src/ui/uas/UASView.h
+++ b/src/ui/uas/UASView.h
@@ -93,6 +93,7 @@ protected:
float lat;
float lon;
float alt;
+ float groundDistance;
void mouseDoubleClickEvent (QMouseEvent * event);
From f2c53477e5521b7897f84d17f2bb4db75eb09d8e Mon Sep 17 00:00:00 2001
From: lm <pixhawk@pixhawk02.(none)>
Date: Wed, 9 Jun 2010 10:24:36 +0200
Subject: [PATCH 2/2] Fixed setting modes
---
src/uas/UAS.cc | 39 ++++++++++----------
src/uas/UAS.h | 2 +-
src/ui/uas/UASControlWidget.cc | 83 ++++++++++++++++++++++++++++--------------
src/ui/uas/UASControlWidget.h | 2 +-
4 files changed, 76 insertions(+), 50 deletions(-)
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 2608af3..2bbc092 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -158,42 +158,42 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if (state.status != this->status)
{
statechanged = true;
- this->status = state.status;
+ this->status = (int)state.status;
getStatusForCode((int)state.status, uasState, stateDescription);
emit statusChanged(this, uasState, stateDescription);
stateAudio = " changed status to " + uasState;
}
- if (static_cast<unsigned int>(this->mode) != static_cast<unsigned int>(state.mode))
+ if (this->mode != static_cast<unsigned int>(state.mode))
{
modechanged = true;
- this->mode = state.mode;
+ this->mode = static_cast<unsigned int>(state.mode);
QString mode;
- switch ((unsigned int)(state.mode))
+ switch (state.mode)
{
- case MAV_MODE_LOCKED:
+ case (uint8_t)MAV_MODE_LOCKED:
mode = "LOCKED MODE";
break;
- case MAV_MODE_MANUAL:
+ case (uint8_t)MAV_MODE_MANUAL:
mode = "MANUAL MODE";
break;
- case MAV_MODE_AUTO:
+ case (uint8_t)MAV_MODE_AUTO:
mode = "AUTO MODE";
break;
- case MAV_MODE_GUIDED:
+ case (uint8_t)MAV_MODE_GUIDED:
mode = "GUIDED MODE";
break;
- case MAV_MODE_READY:
+ case (uint8_t)MAV_MODE_READY:
mode = "READY";
break;
- case MAV_MODE_TEST1:
+ case (uint8_t)MAV_MODE_TEST1:
mode = "TEST1 MODE";
break;
- case MAV_MODE_TEST2:
+ case (uint8_t)MAV_MODE_TEST2:
mode = "TEST2 MODE";
break;
- case MAV_MODE_TEST3:
+ case (uint8_t)MAV_MODE_TEST3:
mode = "TEST3 MODE";
break;
default:
@@ -454,13 +454,12 @@ quint64 UAS::getUnixTime(quint64 time)
void UAS::setMode(int mode)
{
- if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
+ if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_TEST3)
{
- this->mode = mode;
mavlink_message_t msg;
- mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, (unsigned char)mode);
+ mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode);
sendMessage(msg);
- qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << mode;
+ qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
}
}
@@ -838,7 +837,7 @@ void UAS::launch()
{
mavlink_message_t msg;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
- mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(int)MAV_ACTION_LAUNCH);
+ mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_LAUNCH);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
@@ -852,7 +851,7 @@ void UAS::enable_motors()
{
mavlink_message_t msg;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
- mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(int)MAV_ACTION_MOTORS_START);
+ mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_MOTORS_START);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
@@ -866,7 +865,7 @@ void UAS::disable_motors()
{
mavlink_message_t msg;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
- mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(int)MAV_ACTION_MOTORS_STOP);
+ mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_MOTORS_STOP);
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
@@ -884,7 +883,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
manualYawAngle = yaw * yawScaling;
manualThrust = thrust * thrustScaling;
- if(mode == MAV_MODE_MANUAL)
+ if(mode == (int)MAV_MODE_MANUAL)
{
mavlink_message_t message;
mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index fdac701..3425a75 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -107,7 +107,7 @@ protected:
double currentVoltage; ///< Voltage currently measured
float lpVoltage; ///< Low-pass filtered voltage
int timeRemaining; ///< Remaining time calculated based on previous and current
- int mode; ///< The current mode of the MAV
+ unsigned int mode; ///< The current mode of the MAV
int status; ///< The current status of the MAV
quint64 onboardTimeOffset;
diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc
index 41375f2..ff2f5be 100644
--- a/src/ui/uas/UASControlWidget.cc
+++ b/src/ui/uas/UASControlWidget.cc
@@ -42,19 +42,32 @@ This file is part of the PIXHAWK project
#include <UAS.h>
//#include <mavlink.h>
+#define CONTROL_MODE_LOCKED "MODE LOCKED"
+#define CONTROL_MODE_MANUAL "MODE MANUAL"
+#define CONTROL_MODE_GUIDED "MODE GUIDED"
+#define CONTROL_MODE_AUTO "MODE AUTO"
+#define CONTROL_MODE_TEST1 "MODE TEST1"
+
+#define CONTROL_MODE_LOCKED_INDEX 2
+#define CONTROL_MODE_MANUAL_INDEX 3
+#define CONTROL_MODE_GUIDED_INDEX 4
+#define CONTROL_MODE_AUTO_INDEX 5
+#define CONTROL_MODE_TEST1_INDEX 6
+
UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
uas(NULL)
{
ui.setupUi(this);
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
- ui.modeComboBox->insertItem(MAV_MODE_LOCKED, "MODE LOCKED");
- ui.modeComboBox->insertItem(MAV_MODE_MANUAL, "MODE MANUAL");
- ui.modeComboBox->insertItem(MAV_MODE_GUIDED, "MODE GUIDED");
- ui.modeComboBox->insertItem(MAV_MODE_AUTO, "MODE AUTO");
- ui.modeComboBox->insertItem(MAV_MODE_TEST1, "MODE TEST1");
- ui.modeComboBox->insertItem(MAV_MODE_TEST2, "MODE TEST2");
- ui.modeComboBox->insertItem(MAV_MODE_TEST3, "MODE TEST3");
+ ui.modeComboBox->insertItem(0, "Select..");
+ ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED);
+ ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL);
+ ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED);
+ ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO);
+ ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1);
+
+ ui.modeComboBox->setCurrentIndex(0);
}
void UASControlWidget::setUAS(UASInterface* uas)
@@ -92,36 +105,48 @@ UASControlWidget::~UASControlWidget() {
void UASControlWidget::setMode(int mode)
{
// Adapt context button mode
- switch (mode)
+ if (mode == CONTROL_MODE_LOCKED_INDEX)
{
- case MAV_MODE_LOCKED:
- break;
- case MAV_MODE_MANUAL:
- break;
- case MAV_MODE_GUIDED:
- break;
- case MAV_MODE_AUTO:
- break;
- case MAV_MODE_TEST1:
- break;
- case MAV_MODE_TEST2:
- break;
- case MAV_MODE_TEST3:
- break;
+ uasMode = (unsigned int)MAV_MODE_LOCKED;
+ ui.modeComboBox->setCurrentIndex(mode);
}
-
- // Set mode on system
- if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
+ else if (mode == CONTROL_MODE_MANUAL_INDEX)
+ {
+ uasMode = (unsigned int)MAV_MODE_MANUAL;
+ ui.modeComboBox->setCurrentIndex(mode);
+ }
+ else if (mode == CONTROL_MODE_GUIDED_INDEX)
{
- uasMode = mode;
+ uasMode = (unsigned int)MAV_MODE_GUIDED;
ui.modeComboBox->setCurrentIndex(mode);
}
- qDebug() << "SET MODE REQUESTED" << mode;
+ else if (mode == CONTROL_MODE_AUTO_INDEX)
+ {
+ uasMode = (unsigned int)MAV_MODE_AUTO;
+ ui.modeComboBox->setCurrentIndex(mode);
+ }
+ else if (mode == CONTROL_MODE_TEST1_INDEX)
+ {
+ uasMode = (unsigned int)MAV_MODE_TEST1;
+ ui.modeComboBox->setCurrentIndex(mode);
+ }
+ else
+ {
+ qDebug() << "ERROR! MODE NOT FOUND";
+ uasMode = 0;
+ }
+
+
+ qDebug() << "SET MODE REQUESTED" << uasMode;
}
void UASControlWidget::transmitMode()
{
- this->uas->setMode(uasMode);
+ if (uasMode != 0)
+ {
+ this->uas->setMode(uasMode);
+ ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(uas->getUASName()));
+ }
}
void UASControlWidget::cycleContextButton()
@@ -139,12 +164,14 @@ void UASControlWidget::cycleContextButton()
ui.controlButton->setText(tr("Stop Engine"));
mav->setMode(MAV_MODE_MANUAL);
mav->enable_motors();
+ ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(uas->getUASName()));
state++;
break;
case 1:
ui.controlButton->setText(tr("Activate Engine"));
mav->setMode(MAV_MODE_LOCKED);
mav->disable_motors();
+ ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(uas->getUASName()));
state = 0;
break;
case 2:
diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h
index efb892e..33ad1e0 100644
--- a/src/ui/uas/UASControlWidget.h
+++ b/src/ui/uas/UASControlWidget.h
@@ -58,7 +58,7 @@ public slots:
protected:
UASInterface* uas;
- int uasMode;
+ unsigned int uasMode;
private:
Ui::uasControl ui;