Training support

src/comm/MAVLinkSimulationLink.cc

@@ -383,7 +383,8 @@ void MAVLinkSimulationLink::mainloop()
         x = x*0.93f + 0.07f*(x+sin(QGC::groundTimeUsecs()) * 0.08f);
         y = y*0.93f + 0.07f*(y+sin(QGC::groundTimeUsecs()) * 0.5f);
         z = z*0.93f + 0.07f*(z+sin(QGC::groundTimeUsecs()*100000) * 0.1f);
-
+        x = 5247273.0f;
+        y = 465955.0f;
 //        x = (x > 5.0f) ? 5.0f : x;
 //        y = (y > 5.0f) ? 5.0f : y;
 //        z = (z > 3.0f) ? 3.0f : z;
@@ -401,7 +402,7 @@ void MAVLinkSimulationLink::mainloop()
         streampointer += bufferlength;
 
         // Send back new position
-        mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, y+z, y, -fabs(z), 0, 0, 0);
+        mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), 0, 0, 0);
         bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);

src/uas/UAS.cc

@@ -210,6 +210,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     case (uint8_t)MAV_MODE_TEST3:
                         mode = "TEST3 MODE";
                         break;
+                    case (uint8_t)MAV_MODE_RC_TRAINING:
+                        mode = "RC TRAINING MODE";
+                        break;
                     default:
                         mode = "UNINIT MODE";
                         break;
@@ -728,7 +731,7 @@ quint64 UAS::getUnixTime(quint64 time)
 
 void UAS::setMode(int mode)
 {
-    if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_TEST3)
+    if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING)
     {
         mavlink_message_t msg;
         mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode);

src/ui/uas/UASControlWidget.cc

@@ -49,6 +49,8 @@ This file is part of the PIXHAWK project
 #define CONTROL_MODE_TEST1  "MODE TEST1"
 #define CONTROL_MODE_TEST2  "MODE TEST2"
 #define CONTROL_MODE_TEST3  "MODE TEST3"
+#define CONTROL_MODE_READY  "MODE TEST3"
+#define CONTROL_MODE_RC_TRAINING  "MODE RC TRAINING"
 
 #define CONTROL_MODE_LOCKED_INDEX 1
 #define CONTROL_MODE_MANUAL_INDEX 2
@@ -57,6 +59,8 @@ This file is part of the PIXHAWK project
 #define CONTROL_MODE_TEST1_INDEX  5
 #define CONTROL_MODE_TEST2_INDEX  6
 #define CONTROL_MODE_TEST3_INDEX  7
+#define CONTROL_MODE_READY_INDEX  8
+#define CONTROL_MODE_RC_TRAINING_INDEX  9
 
 UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
 uas(0),
@@ -74,6 +78,8 @@ engineOn(false)
     ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1);
     ui.modeComboBox->insertItem(CONTROL_MODE_TEST2_INDEX, CONTROL_MODE_TEST2);
     ui.modeComboBox->insertItem(CONTROL_MODE_TEST3_INDEX, CONTROL_MODE_TEST3);
+    ui.modeComboBox->insertItem(CONTROL_MODE_READY_INDEX, CONTROL_MODE_READY);
+    ui.modeComboBox->insertItem(CONTROL_MODE_RC_TRAINING_INDEX, CONTROL_MODE_RC_TRAINING);
     connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
     connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));
 
@@ -212,6 +218,11 @@ void UASControlWidget::setMode(int mode)
         uasMode = (unsigned int)MAV_MODE_TEST3;
         ui.modeComboBox->setCurrentIndex(mode);
     }
+    else if (mode == CONTROL_MODE_RC_TRAINING_INDEX)
+    {
+        uasMode = (unsigned int)MAV_MODE_RC_TRAINING;
+        ui.modeComboBox->setCurrentIndex(mode);
+    }
     else
     {
         qDebug() << "ERROR! MODE NOT FOUND";