Skip to content
GitLab
Commit 8541598f authored by Anton Babushkin's avatar Anton Babushkin
Browse files

base_mode/custom mode support

parent 0a28c845
Show whitespace changes
Inline Side-by-side
src/uas/UAS.cc
View file @ 8541598f
......@@ -56,9 +56,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
airframe(QGC_AIRFRAME_GENERIC),
autopilot(-1),
systemIsArmed(false),
mode(-1),
base_mode(0),
custom_mode(0),
// custom_mode not initialized
navMode(-1),
status(-1),
// shortModeText not initialized
// shortStateText not initialized
......@@ -335,7 +335,7 @@ void UAS::updateState()
}
else
{
if (((mode&MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (mode&MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
{
GAudioOutput::instance()->notifyNegative();
}
......@@ -566,24 +566,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
stateAudio = uasState;
}
if (this->mode != static_cast<int>(state.base_mode))
if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode)
{
modechanged = true;
this->mode = static_cast<int>(state.base_mode);
shortModeText = getShortModeTextFor(this->mode);
this->base_mode = state.base_mode;
this->custom_mode = state.custom_mode;
shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode, this->autopilot);
emit modeChanged(this->getUASID(), shortModeText, "");
modeAudio = " is now in " + audiomodeText;
}
if (navMode != state.custom_mode)
{
emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode));
navMode = state.custom_mode;
//navModeAudio = tr(" changed nav mode to ") + tr("FIXME");
}
// AUDIO
if (modechanged && statechanged)
{
......@@ -593,7 +587,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
else if (modechanged || statechanged)
{
// Output the one message
audiostring += modeAudio + stateAudio + navModeAudio;
audiostring += modeAudio + stateAudio;
}
if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY))
......@@ -1952,25 +1946,24 @@ QList<int> UAS::getComponentIds()
/**
* @param mode that UAS is to be set to.
*/
void UAS::setMode(int mode)
void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode)
{
//this->mode = mode; //no call assignament, update receive message from UAS
// Strip armed / disarmed call, this is not relevant for setting the mode
uint8_t newMode = mode;
newMode &= (~(uint8_t)MAV_MODE_FLAG_SAFETY_ARMED);
newBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
// Now set current state (request no change)
newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_SAFETY_ARMED);
newBaseMode |= this->base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
// Strip HIL part, replace it with current system state
newMode &= (~(uint8_t)MAV_MODE_FLAG_HIL_ENABLED);
newBaseMode &= (~MAV_MODE_FLAG_HIL_ENABLED);
// Now set current state (request no change)
newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_HIL_ENABLED);
newBaseMode |= this->base_mode & MAV_MODE_FLAG_HIL_ENABLED;
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newMode, (uint16_t)navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode);
sendMessage(msg);
qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newMode;
qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newBaseMode;
}
/**
......@@ -2054,35 +2047,6 @@ float UAS::filterVoltage(float value) const
return lpVoltage * 0.7f + value * 0.3f;
}
/**
* The mode can be preflight or unknown.
* @Return the mode of the autopilot
*/
QString UAS::getNavModeText(int mode)
{
if (autopilot == MAV_AUTOPILOT_PIXHAWK)
{
switch (mode)
{
case 0:
return QString("PREFLIGHT");
break;
default:
return QString("UNKNOWN");
}
}
else if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA)
{
return QString("UNKNOWN");
}
else if (autopilot == MAV_AUTOPILOT_OPENPILOT)
{
return QString("UNKNOWN");
}
// If nothing matches, return unknown
return QString("UNKNOWN");
}
/**
* Get the status of the code and a description of the status.
* Status can be unitialized, booting up, calibrating sensors, active
......@@ -2738,7 +2702,7 @@ void UAS::launch()
void UAS::armSystem()
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_SAFETY_ARMED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
sendMessage(msg);
}
......@@ -2749,35 +2713,35 @@ void UAS::armSystem()
void UAS::disarmSystem()
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & ~MAV_MODE_FLAG_SAFETY_ARMED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode & ~MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
sendMessage(msg);
}
void UAS::toggleArmedState()
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_SAFETY_ARMED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode ^ MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
sendMessage(msg);
}
void UAS::goAutonomous()
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & MAV_MODE_FLAG_AUTO_ENABLED & ~MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_MODE_FLAG_AUTO_ENABLED, 0);
sendMessage(msg);
}
void UAS::goManual()
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & ~MAV_MODE_FLAG_AUTO_ENABLED & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
sendMessage(msg);
}
void UAS::toggleAutonomy()
{
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
sendMessage(msg);
}
......@@ -2801,7 +2765,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
manualThrust = thrust * thrustScaling;
// If system has manual inputs enabled and is armed
if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{
mavlink_message_t message;
mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons);
......@@ -2819,7 +2783,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{
// If system has manual inputs enabled and is armed
if(((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (mode & MAV_MODE_FLAG_HIL_ENABLED))
if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{
mavlink_message_t message;
mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw);
......@@ -3098,7 +3062,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
float pitchspeed, float yawspeed, double lat, double lon, double alt,
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
{
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{
float q[4];
......@@ -3127,7 +3091,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
{
// Attempt to set HIL mode
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
......@@ -3136,7 +3100,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{
mavlink_message_t msg;
mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
......@@ -3150,7 +3114,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
{
// Attempt to set HIL mode
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
......@@ -3162,7 +3126,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
return;
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
{
float course = cog;
// map to 0..2pi
......@@ -3181,7 +3145,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
{
// Attempt to set HIL mode
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
......@@ -3197,7 +3161,7 @@ void UAS::startHil()
hilEnabled = true;
sensorHil = false;
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg);
// Connect HIL simulation link
simulation->connectSimulation();
......@@ -3210,7 +3174,7 @@ void UAS::stopHil()
{
if (simulation) simulation->disconnectSimulation();
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), base_mode & !MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
sendMessage(msg);
hilEnabled = false;
sensorHil = false;
......@@ -3338,51 +3302,57 @@ QString UAS::getAudioModeTextFor(int id)
* The mode returned can be auto, stabilized, test, manual, preflight or unknown.
* @return the short text of the mode for the id given.
*/
QString UAS::getShortModeTextFor(int id)
QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot)
{
QString mode = "";
uint8_t modeid = id;
// BASE MODE DECODING
if (modeid == 0)
{
mode = "|PREFLIGHT";
}
else {
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO){
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO
};
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
// use custom_mode - autopilot-specific
if (autopilot == MAV_AUTOPILOT_PX4) {
if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
mode += "|MANUAL";
} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
mode += "|SEATBELT";
} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
mode += "|EASY";
} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
mode += "|AUTO";
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL){
mode += "|MANUAL";
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED){
mode += "|VECTOR";
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE){
// fallback to using base_mode
if (mode.length() == 0) {
// use base_mode - not autopilot-specific
if (base_mode == 0) {
mode += "|PREFLIGHT";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) {
mode += "|AUTO";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) {
mode += "|MANUAL";
if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) {
mode += "|GUIDED";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) {
mode += "|STABILIZED";
}
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_TEST){
mode += "|TEST";
}
}
if (mode.length() == 0)
{
mode = "|UNKNOWN";
qDebug() << __FILE__ << __LINE__ << " Unknown modeid: " << modeid;
qDebug() << __FILE__ << __LINE__ << " Unknown mode: base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot;
}
// ARMED STATE DECODING
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
{
mode.prepend("A");
}
......@@ -3392,12 +3362,12 @@ QString UAS::getShortModeTextFor(int id)
}
// HARDWARE IN THE LOOP DECODING
if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
{
mode.prepend("HIL:");
}
qDebug() << "MODE: " << modeid << " " << mode;
//qDebug() << "base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot << ": " << mode;
return mode;
}
......
src/uas/UAS.h
View file @ 8541598f
......@@ -68,7 +68,7 @@ public:
/** @brief Get short mode */
const QString& getShortMode() const;
/** @brief Translate from mode id to text */
static QString getShortModeTextFor(int id);
static QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot);
/** @brief Translate from mode id to audio text */
static QString getAudioModeTextFor(int id);
/** @brief Get the unique system id */
......@@ -361,9 +361,8 @@ protected: //COMMENTS FOR TEST UNIT
int airframe; ///< The airframe type
int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
bool systemIsArmed; ///< If the system is armed
uint8_t mode; ///< The current mode of the MAV
uint8_t base_mode; ///< The current mode of the MAV
uint32_t custom_mode; ///< The current mode of the MAV
uint32_t navMode; ///< The current navigation mode of the MAV
int status; ///< The current status of the MAV
QString shortModeText; ///< Short textual mode description
QString shortStateText; ///< Short textual state description
......@@ -505,8 +504,6 @@ public:
float getChargeLevel();
/** @brief Get the human-readable status message for this code */
void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);
/** @brief Get the human-readable navigation mode translation for this mode */
QString getNavModeText(int mode);
/** @brief Check if vehicle is in autonomous mode */
bool isAuto();
/** @brief Check if vehicle is armed */
......@@ -833,7 +830,7 @@ public slots:
void setSelected();
/** @brief Set current mode of operation, e.g. auto or manual */
void setMode(int mode);
void setMode(uint8_t newBaseMode, uint32_t newCustomMode);
/** @brief Request all parameters */
void requestParameters();
......
src/uas/UASInterface.h
View file @ 8541598f
......@@ -296,7 +296,7 @@ public slots:
/** @brief Start/continue the current robot action */
virtual void go() = 0;
/** @brief Set the current mode of operation */
virtual void setMode(int mode) = 0;
virtual void setMode(uint8_t newBaseMode, uint32_t newCustomMode) = 0;
/** Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/
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 **/
......
src/ui/uas/UASControlWidget.cc
View file @ 8541598f
......@@ -33,7 +33,6 @@ This file is part of the PIXHAWK project
#include <QTimer>
#include <QLabel>
#include <QFileDialog>
#include <QDebug>
#include <QProcess>
#include <QPalette>
......@@ -42,71 +41,111 @@ This file is part of the PIXHAWK project
#include <UAS.h>
#include "QGC.h"
static struct full_mode_s modes_list_common[] = {
{ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
0 },
{ (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
0 },
{ (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
0 },
{ (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
0 },
};
static struct full_mode_s modes_list_px4[] = {
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
PX4_CUSTOM_MODE_MANUAL },
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
PX4_CUSTOM_MODE_SEATBELT },
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
PX4_CUSTOM_MODE_EASY },
{ (MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
PX4_CUSTOM_MODE_AUTO },
};
UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
uas(0),
uasMode(0),
engineOn(false)
uasID(-1),
modesList(NULL),
modesNum(0),
modeIdx(0),
armed(false)
{
ui.setupUi(this);
this->setUAS(UASManager::instance()->getActiveUAS());
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
ui.modeComboBox->clear();
int modes[] = {
MAV_MODE_PREFLIGHT,
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED,
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED,
};
for (int i = 0; i < sizeof(modes) / sizeof(int); i++) {
int mode = modes[i];
ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode).remove(0, 2), mode);
}
connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
uasMode = ui.modeComboBox->itemData(ui.modeComboBox->currentIndex()).toInt();
ui.gridLayout->setAlignment(Qt::AlignTop);
}
void UASControlWidget::updateModesList()
{
// Detect autopilot type
int autopilot = 0;
if (this->uasID >= 0) {
UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
if (uas) {
autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
}
}
ui.modeComboBox->setCurrentIndex(0);
// Use corresponding modes list
if (autopilot == MAV_AUTOPILOT_PX4) {
modesList = modes_list_px4;
modesNum = sizeof(modes_list_px4) / sizeof(struct full_mode_s);
} else {
modesList = modes_list_common;
modesNum = sizeof(modes_list_common) / sizeof(struct full_mode_s);
}
ui.gridLayout->setAlignment(Qt::AlignTop);
// Set combobox items
ui.modeComboBox->clear();
for (int i = 0; i < modesNum; i++) {
struct full_mode_s mode = modesList[i];
ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode.baseMode, mode.customMode, autopilot).remove(0, 2), i);
}
// Select first mode in list
modeIdx = 0;
ui.modeComboBox->setCurrentIndex(modeIdx);
ui.modeComboBox->update();
}
void UASControlWidget::setUAS(UASInterface* uas)
{
if (this->uas)
{
UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas);
if (this->uasID) {
UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uasID);
disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem()));
disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch()));
disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home()));
disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown()));
//connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int, QString, QString)));
disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
}
// Connect user interface controls
if (uas)
{
if (uas) {
connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
//connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(uas, SIGNAL(modeChanged(int, QString, QString)), this, SLOT(updateMode(int, QString, QString)));
connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
this->uas = uas->getUASID();
this->uasID = uas->getUASID();
setBackgroundColor(uas->getColor());
} else {
this->uasID = -1;
}
else
{
this->uas = -1;
}
this->updateModesList();
this->updateArmText();
}
UASControlWidget::~UASControlWidget()
......@@ -114,15 +153,11 @@ UASControlWidget::~UASControlWidget()
}
void UASControlWidget::updateStatemachine()
void UASControlWidget::updateArmText()
{
if (engineOn)
{
if (armed) {
ui.controlButton->setText(tr("DISARM SYSTEM"));
}
else
{
} else {
ui.controlButton->setText(tr("ARM SYSTEM"));
}
}
......@@ -138,7 +173,7 @@ void UASControlWidget::setBackgroundColor(QColor color)
QString colorstyle;
QString borderColor = "#4A4A4F";
borderColor = "#FA4A4F";
uasColor = uasColor.darker(900);
uasColor = uasColor.darker(400);
colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }",
uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str());
setStyleSheet(colorstyle);
......@@ -149,7 +184,7 @@ void UASControlWidget::setBackgroundColor(QColor color)
}
void UASControlWidget::updateMode(int uas,QString mode,QString description)
void UASControlWidget::updateMode(int uas, QString mode, QString description)
{
Q_UNUSED(uas);
Q_UNUSED(mode);
......@@ -158,17 +193,15 @@ void UASControlWidget::updateMode(int uas,QString mode,QString description)
void UASControlWidget::updateState(int state)
{
switch (state)
{
switch (state) {
case (int)MAV_STATE_ACTIVE:
engineOn = true;
ui.controlButton->setText(tr("DISARM SYSTEM"));
armed = true;
break;
case (int)MAV_STATE_STANDBY:
engineOn = false;
ui.controlButton->setText(tr("ARM SYSTEM"));
armed = false;
break;
}
this->updateArmText();
}
/**
......@@ -177,7 +210,7 @@ void UASControlWidget::updateState(int state)
void UASControlWidget::setMode(int mode)
{
// Adapt context button mode
uasMode = ui.modeComboBox->itemData(mode).toInt();
modeIdx = mode;
ui.modeComboBox->blockSignals(true);
ui.modeComboBox->setCurrentIndex(mode);
ui.modeComboBox->blockSignals(false);
......@@ -187,43 +220,36 @@ void UASControlWidget::setMode(int mode)
void UASControlWidget::transmitMode()
{
UASInterface* mav = UASManager::instance()->getUASForId(this->uas);
if (mav)
{
UASInterface* uas = UASManager::instance()->getUASForId(this->uasID);
if (uas) {
if (modeIdx >= 0 && modeIdx < modesNum) {
struct full_mode_s mode = modesList[modeIdx];
// include armed state
if (engineOn)
uasMode |= MAV_MODE_FLAG_SAFETY_ARMED;
else
uasMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
if (armed) {
mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
mav->setMode(uasMode);
QString mode = ui.modeComboBox->currentText();
uas->setMode(mode.baseMode, mode.customMode);
QString modeText = ui.modeComboBox->currentText();
ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(mode).arg(mav->getUASName()));
ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName()));
}
}
}
void UASControlWidget::cycleContextButton()
{
UAS* mav = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uas));
if (mav)
{
if (!engineOn)
{
mav->armSystem();
ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName()));
UAS* uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(this->uasID));
if (uas) {
if (!armed) {
uas->armSystem();
ui.lastActionLabel->setText(QString("Arm %1").arg(uas->getUASName()));
} else {
mav->disarmSystem();
ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName()));
uas->disarmSystem();
ui.lastActionLabel->setText(QString("Disarm %1").arg(uas->getUASName()));
}
// Update state now and in several intervals when MAV might have changed state
updateStatemachine();
QTimer::singleShot(50, this, SLOT(updateStatemachine()));
QTimer::singleShot(200, this, SLOT(updateStatemachine()));
}
}
src/ui/uas/UASControlWidget.h
View file @ 8541598f
......@@ -39,6 +39,18 @@ This file is part of the QGROUNDCONTROL project
#include <ui_UASControl.h>
#include <UASInterface.h>
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO
};
struct full_mode_s {
uint8_t baseMode;
uint32_t customMode;
};
/**
* @brief Widget controlling one MAV
*/
......@@ -51,8 +63,10 @@ public:
~UASControlWidget();
public slots:
/** @brief Update modes list for selected system */
void updateModesList();
/** @brief Set the system this widget controls */
void setUAS(UASInterface* uas);
void setUAS(UASInterface* uasID);
/** @brief Trigger next context action */
void cycleContextButton();
/** @brief Set the operation mode of the MAV */
......@@ -60,11 +74,11 @@ public slots:
/** @brief Transmit the operation mode */
void transmitMode();
/** @brief Update the mode */
void updateMode(int uas,QString mode,QString description);
void updateMode(int uasID, QString mode, QString description);
/** @brief Update state */
void updateState(int state);
/** @brief Update internal state machine */
void updateStatemachine();
void updateArmText();
signals:
void changedMode(int);
......@@ -75,9 +89,11 @@ protected slots:
void setBackgroundColor(QColor color);
protected:
int uas; ///< Reference to the current uas
unsigned int uasMode; ///< Current uas mode
bool engineOn; ///< Engine state
int uasID; ///< Reference to the current uas
struct full_mode_s *modesList; ///< Modes list for the current UAS
int modesNum; ///< Number of modes in list for the current UAS
int modeIdx; ///< Current uas mode index
bool armed; ///< Engine state
private:
Ui::uasControl ui;
......
Supports Markdown
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