zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge branch 'master' of git@pixhawk.ethz.ch:qgroundcontrol

QGC4.4
pixhawk 15 years ago
parent
d422adade2 f2c53477e5
commit
227edfeb50
6 changed files with 140 additions and 56 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 64
      src/uas/UAS.cc
  2. 2
      src/uas/UAS.h
  3. 83
      src/ui/uas/UASControlWidget.cc
  4. 2
      src/ui/uas/UASControlWidget.h
  5. 44
      src/ui/uas/UASView.cc
  6. 1
      src/ui/uas/UASView.h

64
src/uas/UAS.cc
Unescape View File

@ -159,42 +159,42 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -159,42 +159,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:
@ -324,14 +324,33 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -324,14 +324,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:
@ -454,13 +473,12 @@ quint64 UAS::getUnixTime(quint64 time) @@ -454,13 +473,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 +856,7 @@ void UAS::launch() @@ -838,7 +856,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 +870,7 @@ void UAS::enable_motors() @@ -852,7 +870,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 +884,7 @@ void UAS::disable_motors() @@ -866,7 +884,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 +902,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double @@ -884,7 +902,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);

2
src/uas/UAS.h
Unescape View File

@ -110,7 +110,7 @@ protected: @@ -110,7 +110,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;

83
src/ui/uas/UASControlWidget.cc
Unescape View File

@ -42,19 +42,32 @@ This file is part of the PIXHAWK project @@ -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() { @@ -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() @@ -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:

2
src/ui/uas/UASControlWidget.h
Unescape View File

@ -58,7 +58,7 @@ public slots: @@ -58,7 +58,7 @@ public slots:
protected:
UASInterface* uas;
int uasMode;
unsigned int uasMode;
private:
Ui::uasControl ui;

44
src/ui/uas/UASView.cc
Unescape View File

@ -40,16 +40,26 @@ This file is part of the PIXHAWK project @@ -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() @@ -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);

1
src/ui/uas/UASView.h
Unescape View File

@ -93,6 +93,7 @@ protected: @@ -93,6 +93,7 @@ protected:
float lat;
float lon;
float alt;
float groundDistance;
void mouseDoubleClickEvent (QMouseEvent * event);

Write Preview
Loading…
Cancel
Save