diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3e2e7a59636b7d5f8d593b88ecc9a0678fe8c801..6e3d4f2136e13ab1cf84a31ca46c91949899e02f 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -286,30 +286,44 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) case (uint8_t)MAV_MODE_MANUAL: mode = "MANUAL MODE"; break; + + #ifdef MAVLINK_ENABLED_SLUGS + case (uint8_t)MAV_MODE_AUTO: + mode = "WAYPOINT MODE"; + break; + case (uint8_t)MAV_MODE_GUIDED: + mode = "MID-L CMDS MODE"; + break; + + case (uint8_t)MAV_MODE_TEST1: + mode = "PASST MODE"; + break; + case (uint8_t)MAV_MODE_TEST2: + mode = "SEL PT MODE"; + break; + #else case (uint8_t)MAV_MODE_AUTO: mode = "AUTO MODE"; break; case (uint8_t)MAV_MODE_GUIDED: mode = "GUIDED MODE"; break; - case (uint8_t)MAV_MODE_READY: - mode = "READY MODE"; - break; + case (uint8_t)MAV_MODE_TEST1: mode = "TEST1 MODE"; break; case (uint8_t)MAV_MODE_TEST2: mode = "TEST2 MODE"; break; - #ifdef MAVLINK_ENABLED_SLUGS - case (uint8_t)MAV_MODE_TEST3: - mode = "HIL MODE"; + #endif + case (uint8_t)MAV_MODE_READY: + mode = "READY MODE"; break; - #else + case (uint8_t)MAV_MODE_TEST3: mode = "TEST3 MODE"; - break; - #endif + break; + case (uint8_t)MAV_MODE_RC_TRAINING: mode = "RC TRAINING MODE"; break; @@ -1404,10 +1418,16 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc uasState = tr("EMERGENCY"); stateDescription = tr("EMERGENCY: Land Immediately!"); break; + case MAV_STATE_HILSIM: + uasState = tr("HIL SIM"); + stateDescription = tr("HIL Simulation, Sensors read from SIM"); + break; + case MAV_STATE_POWEROFF: uasState = tr("SHUTDOWN"); stateDescription = tr("Powering off system."); break; + default: uasState = tr("UNKNOWN"); stateDescription = tr("Unknown system state"); @@ -2000,12 +2020,14 @@ void UAS::clearWaypointList() void UAS::halt() { + 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(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); + } void UAS::go() @@ -2035,6 +2057,7 @@ void UAS::home() */ void UAS::emergencySTOP() { + 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(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); @@ -2077,6 +2100,29 @@ bool UAS::emergencyKILL() return result; } +void UAS::startHil(){ + + 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(), MAV_COMP_ID_IMU,(int)MAV_ACTION_START_HILSIM); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); + +} + +void UAS::stopHil(){ + + 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(), MAV_COMP_ID_IMU,(int)MAV_ACTION_STOP_HILSIM); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); + +} + + void UAS::shutdown() { bool result = false; @@ -2084,6 +2130,7 @@ void UAS::shutdown() msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Shutting down the UAS"); msgBox.setInformativeText("Do you want to shut down the onboard computer?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Cancel); int ret = msgBox.exec(); @@ -2101,6 +2148,7 @@ void UAS::shutdown() // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); + result = true; } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 3ff775faeb914e95b15ccb46f04684600b246da7..e39531619dd091968aecbe20058bd54f578c5802 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -233,6 +233,13 @@ public slots: void home(); void halt(); void go(); + /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/ + void startHil(); + + /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/ + void stopHil(); + + /** @brief Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/ void emergencySTOP(); diff --git a/src/ui/SlugsHilSim.cc b/src/ui/SlugsHilSim.cc index 11f390b20a737f184b96da7c304ce7c625d67851..31b3cdc936b150d9d97f5c4fa2b1a65e096cc240 100644 --- a/src/ui/SlugsHilSim.cc +++ b/src/ui/SlugsHilSim.cc @@ -108,6 +108,8 @@ void SlugsHilSim::putInHilMode(void){ ui->bt_startHil->setText(buttonCaption); + activeUas->startHil(); + } else { ui->bt_startHil->setChecked(false); } @@ -120,6 +122,7 @@ void SlugsHilSim::putInHilMode(void){ ui->bt_startHil->setText(buttonCaption); rxSocket->disconnectFromHost(); + activeUas->stopHil(); } } @@ -151,8 +154,6 @@ void SlugsHilSim::activeUasSet(UASInterface* uas){ if (uas != NULL) { activeUas = static_cast (uas); - - //connect(uas, SIGNAL()) } } diff --git a/src/ui/uas/UASControlParameters.cpp b/src/ui/uas/UASControlParameters.cpp index a180334db6eb00dfebb3180ddc451de506328084..f04d77f29b45b0a68cc46945dda0c239ce4610b3 100644 --- a/src/ui/uas/UASControlParameters.cpp +++ b/src/ui/uas/UASControlParameters.cpp @@ -3,11 +3,19 @@ #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_TEST2 "MODE TEST2" -#define CONTROL_MODE_TEST3 "MODE TEST3" + +#ifdef MAVLINK_ENABLED_SLUGS + #define CONTROL_MODE_GUIDED "MODE MID-L CMDS" + #define CONTROL_MODE_AUTO "MODE WAYPOINT" + #define CONTROL_MODE_TEST1 "MODE PASST" + #define CONTROL_MODE_TEST2 "MODE SEL PT" +#else + #define CONTROL_MODE_GUIDED "MODE GUIDED" + #define CONTROL_MODE_AUTO "MODE AUTO" + #define CONTROL_MODE_TEST1 "MODE TEST1" + #define CONTROL_MODE_TEST2 "MODE TEST2" +#endif + #define CONTROL_MODE_READY "MODE TEST3" #define CONTROL_MODE_RC_TRAINING "RC SIMULATION" @@ -51,20 +59,38 @@ void UASControlParameters::changedMode(int mode) case (uint8_t)MAV_MODE_MANUAL: modeTemp = "MANUAL MODE"; break; +#ifdef MAVLINK_ENABLED_SLUGS case (uint8_t)MAV_MODE_AUTO: - modeTemp = "AUTO MODE"; + modeTemp = "WAYPOINT MODE"; break; case (uint8_t)MAV_MODE_GUIDED: - modeTemp = "GUIDED MODE"; + modeTemp = "MID-L CMDS MODE"; break; - case (uint8_t)MAV_MODE_READY: - modeTemp = "READY MODE"; + + case (uint8_t)MAV_MODE_TEST1: + modeTemp = "PASST MODE"; + break; + case (uint8_t)MAV_MODE_TEST2: + modeTemp = "SEL PT MODE"; break; +#else + case (uint8_t)MAV_MODE_AUTO: + mode = "AUTO MODE"; + break; + case (uint8_t)MAV_MODE_GUIDED: + mode = "GUIDED MODE"; + break; + case (uint8_t)MAV_MODE_TEST1: - modeTemp = "TEST1 MODE"; + mode = "TEST1 MODE"; break; case (uint8_t)MAV_MODE_TEST2: - modeTemp = "TEST2 MODE"; + mode = "TEST2 MODE"; + break; +#endif + case (uint8_t)MAV_MODE_READY: + modeTemp = "READY MODE"; + break; break; case (uint8_t)MAV_MODE_TEST3: modeTemp = "TEST3 MODE"; diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index cbba86908e2cd9912b87811fae0a444054e1c485..278a5328bb2eb48f5a785bcaff1fc2935971ccfe 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -45,16 +45,21 @@ This file is part of the PIXHAWK project #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_TEST2 "MODE TEST2" + #ifdef MAVLINK_ENABLED_SLUGS - #define CONTROL_MODE_TEST3 "MODE HIL" + #define CONTROL_MODE_GUIDED "MODE MID-L CMDS" + #define CONTROL_MODE_AUTO "MODE WAYPOINT" + #define CONTROL_MODE_TEST1 "MODE PASST" + #define CONTROL_MODE_TEST2 "MODE SEL PT" #else - #define CONTROL_MODE_TEST3 "MODE TEST3" + #define CONTROL_MODE_GUIDED "MODE GUIDED" + #define CONTROL_MODE_AUTO "MODE AUTO" + #define CONTROL_MODE_TEST1 "MODE TEST1" + #define CONTROL_MODE_TEST2 "MODE TEST2" #endif -#define CONTROL_MODE_READY "MODE TEST3" + +#define CONTROL_MODE_TEST3 "MODE TEST3" +#define CONTROL_MODE_READY "MODE READY" #define CONTROL_MODE_RC_TRAINING "RC SIMULATION" #define CONTROL_MODE_LOCKED_INDEX 1 @@ -91,6 +96,7 @@ engineOn(false) ui.modeComboBox->setCurrentIndex(0); ui.gridLayout->setAlignment(Qt::AlignTop); + } void UASControlWidget::setUAS(UASInterface* uas)