Fixed setting modes

src/uas/UAS.cc

@@ -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

@@ -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

@@ -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 = (unsigned int)MAV_MODE_GUIDED;
+        ui.modeComboBox->setCurrentIndex(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 = mode;
+        uasMode = (unsigned int)MAV_MODE_TEST1;
         ui.modeComboBox->setCurrentIndex(mode);
     }
-    qDebug() << "SET MODE REQUESTED" << mode;
+    else
+    {
+        qDebug() << "ERROR! MODE NOT FOUND";
+        uasMode = 0;
+    }
+
+
+    qDebug() << "SET MODE REQUESTED" << uasMode;
 }
 
 void UASControlWidget::transmitMode()
 {
+    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

@@ -58,7 +58,7 @@ public slots:
 
 protected:
     UASInterface* uas;
-    int uasMode;
+    unsigned int uasMode;
 
 private:
     Ui::uasControl ui;