Skip to content

Q
qgroundcontrol
Commit f2c53477 authored by lm's avatar lm
Browse files
Options

Fixed setting modes

parent 06a542bf
Hide whitespace changes
Inline Side-by-side
Showing with 76 additions and 50 deletions
src/uas/UAS.cc
View file @ f2c53477
......@@ -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);
......
src/uas/UAS.h
View file @ f2c53477
......@@ -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;
......
src/ui/uas/UASControlWidget.cc
View file @ f2c53477
......@@ -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:
......
src/ui/uas/UASControlWidget.h
View file @ f2c53477
......@@ -58,7 +58,7 @@ public slots:
protected:
UASInterface* uas;
int uasMode;
unsigned int uasMode;
private:
Ui::uasControl ui;
......
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