diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 457b294e0bd3c4b9ec947f8b5dc6c9faf734eb57..b2580593763b44277f162e2c5b1ad1a5b1e95624 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -57,9 +57,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 @@ -337,7 +337,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(); } @@ -568,24 +568,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) stateAudio = uasState; } - if (this->mode != static_cast(state.base_mode)) + if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode) { modechanged = true; - this->mode = static_cast(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) { @@ -595,7 +589,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)) @@ -1826,25 +1820,24 @@ QList 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; } /** @@ -1945,35 +1938,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 @@ -2750,7 +2714,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); } @@ -2761,35 +2725,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); } @@ -2813,7 +2777,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); @@ -2831,7 +2795,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); @@ -3110,7 +3074,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]; @@ -3139,7 +3103,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."; } @@ -3148,7 +3112,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, @@ -3162,7 +3126,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."; } @@ -3174,7 +3138,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 @@ -3193,7 +3157,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."; } @@ -3209,7 +3173,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(); @@ -3222,7 +3186,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; @@ -3350,51 +3314,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){ - mode += "|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){ + // 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_GUIDED){ - mode += "|VECTOR"; - } - - if (modeid & (uint8_t)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"); } @@ -3404,12 +3374,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; } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 0d89a75e33cd89642aa4d00cb80d5cf779574ab9..02d8159aee2d264f4f2e4a2d2cc5482bb6d9bcef 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -69,7 +69,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 */ @@ -362,9 +362,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 @@ -506,8 +505,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 */ @@ -834,7 +831,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(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 8714ad3c433acc38ec410c861022e7982e9c0ad0..7a0640eddd9aba61bbadfc07dd87e3d45bb7a519 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -295,7 +295,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 **/ diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index b7d8cf798144797df42414bcb927d3a2e4a02dc9..aae864499177b8e089fba0510bf4e8e8c299dc72 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -33,7 +33,6 @@ This file is part of the PIXHAWK project #include #include #include -#include #include #include @@ -42,71 +41,111 @@ This file is part of the PIXHAWK project #include #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); +} - ui.modeComboBox->setCurrentIndex(0); +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.gridLayout->setAlignment(Qt::AlignTop); + // 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); + } + + // 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) - { - 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(statusChanged(int)), this, SLOT(updateState(int))); - - ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); - - this->uas = uas->getUASID(); - setBackgroundColor(uas->getColor()); - } - else - { - this->uas = -1; - } + 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(statusChanged(int)), this, SLOT(updateState(int))); + + ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); + + this->uasID = uas->getUASID(); + setBackgroundColor(uas->getColor()); + } else { + this->uasID = -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) - { - // include armed state - if (engineOn) - uasMode |= MAV_MODE_FLAG_SAFETY_ARMED; - else - uasMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - - mav->setMode(uasMode); - QString mode = ui.modeComboBox->currentText(); - - ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(mode).arg(mav->getUASName())); + 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 (armed) { + mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED; + } else { + mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + } + + uas->setMode(mode.baseMode, mode.customMode); + QString modeText = ui.modeComboBox->currentText(); + + ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName())); + } } } void UASControlWidget::cycleContextButton() { - UAS* mav = dynamic_cast(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(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())); - } - } diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h index 4ec60f6930fefe0bbfc90369bdb708e92acec918..7f9a5652b74c00c92ee13a6780ca4ff94d617d40 100644 --- a/src/ui/uas/UASControlWidget.h +++ b/src/ui/uas/UASControlWidget.h @@ -39,6 +39,18 @@ This file is part of the QGROUNDCONTROL project #include #include +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;