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

Fixed setting modes

QGC4.4
lm 15 years ago
parent
06a542bf06
commit
f2c53477e5
4 changed files with 76 additions and 50 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. 39
      src/uas/UAS.cc
  2. 2
      src/uas/UAS.h
  3. 83
      src/ui/uas/UASControlWidget.cc
  4. 2
      src/ui/uas/UASControlWidget.h

39
src/uas/UAS.cc
Unescape View File

@ -158,42 +158,42 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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 @@ -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);

2
src/uas/UAS.h
Unescape View File

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

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;

Write Preview
Loading…
Cancel
Save