diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c
index 797a6cf1c..2546f3a90 100644
--- a/flight/modules/ManualControl/armhandler.c
+++ b/flight/modules/ManualControl/armhandler.c
@@ -90,11 +90,11 @@ void armHandler(bool newinit)
         armSwitch = true;
         break;
     case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
-        AccessoryDesiredInstGet(0, &acc);
+        AccessoryDesiredInstGet(1, &acc);
         armSwitch = true;
         break;
     case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
-        AccessoryDesiredInstGet(0, &acc);
+        AccessoryDesiredInstGet(2, &acc);
         armSwitch = true;
         break;
     default:
@@ -298,7 +298,7 @@ static bool forcedDisArm(void)
     if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
         return true;
     }
-    if (alarms.ManualControl == SYSTEMALARMS_ALARM_CRITICAL) {
+    if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
         return true;
     }
     return false;
diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c
index 7534eba47..95c06de72 100644
--- a/flight/modules/ManualControl/manualcontrol.c
+++ b/flight/modules/ManualControl/manualcontrol.c
@@ -193,7 +193,7 @@ static void manualControlTask(void)
 
     // Depending on the mode update the Stabilization or Actuator objects
     controlHandler *handler = &handler_MANUAL;
-    switch (flightStatus.FlightMode) {
+    switch (newMode) {
     case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
         handler = &handler_MANUAL;
         break;
@@ -223,7 +223,12 @@ static void manualControlTask(void)
     }
 
     bool newinit = false;
-    if (flightStatus.FlightMode != newMode) {
+
+    // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
+    static bool firstRun = true;
+
+    if (flightStatus.FlightMode != newMode || firstRun) {
+        firstRun = false;
         flightStatus.ControlChain = handler->controlChain;
         flightStatus.FlightMode   = newMode;
         FlightStatusSet(&flightStatus);
diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
index 0d2889785..fa1e206d9 100644
--- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
+++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
@@ -52,12 +52,12 @@
       </widget>
      </item>
      <item>
-      <widget class="QCheckBox" name="checkBoxArmed">
+      <widget class="QCheckBox" name="checkBoxArming">
        <property name="enabled">
         <bool>true</bool>
        </property>
        <property name="text">
-        <string>Armed</string>
+        <string>Arm switch (Accessory0)</string>
        </property>
       </widget>
      </item>
diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
index ce2629bba..d3b358ce6 100644
--- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
+++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
@@ -231,7 +231,9 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r
         }
         if (buttonThrottleControl == 0) {
             obj->getField("Throttle")->setDouble(newThrottle);
+            obj->getField("Thrust")->setDouble(newThrottle);
         }
+        obj->getField("Connected")->setValue("True");
         obj->updated();
     }
 }
@@ -308,9 +310,11 @@ void GCSControlGadget::readUDPCommand()
             }
             if (throttle != obj->getField("Throttle")->getDouble()) {
                 obj->getField("Throttle")->setDouble(constrain(throttle));
+                obj->getField("Thrust")->setDouble(constrain(throttle));
                 update = true;
             }
             if (update) {
+                obj->getField("Connected")->setValue("True");
                 obj->updated();
             }
         }
@@ -354,6 +358,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
                     break;
                 case 4: // Throttle
                     obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() + buttonSettings[number].Amount));
+                    obj->getField("Thrust")->setValue(bound(obj->getField("Thrust")->getValue().toDouble() + buttonSettings[number].Amount));
                     break;
                 }
             }
@@ -372,6 +377,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
                     break;
                 case 4: // Throttle
                     obj->getField("Throttle")->setValue(bound(obj->getField("Throttle")->getValue().toDouble() - buttonSettings[number].Amount));
+                    obj->getField("Thrust")->setValue(bound(obj->getField("Thrust")->getValue().toDouble() - buttonSettings[number].Amount));
                     break;
                 }
             }
@@ -380,14 +386,10 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
             switch (buttonSettings[number].FunctionID) {
             case 1: // Armed
                 if (currentCGSControl) {
-                    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
-                    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
-                    UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
-
-                    if (obj->getField("Armed")->getValue().toString().compare("Armed") == 0) {
-                        obj->getField("Armed")->setValue("Disarmed");
+                    if (((GCSControlGadgetWidget *)m_widget)->getArmed()) {
+                        ((GCSControlGadgetWidget *)m_widget)->setArmed(false);
                     } else {
-                        obj->getField("Armed")->setValue("Armed");
+                        ((GCSControlGadgetWidget *)m_widget)->setArmed(true);
                     }
                 }
                 break;
@@ -407,6 +409,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
             break;
         }
 
+        obj->getField("Connected")->setValue("True");
         obj->updated();
     }
     // buttonSettings[number].ActionID NIDT
diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
index 55dcf8dbc..cd9d05489 100644
--- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
@@ -53,8 +53,13 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
     m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY);
 
     // Set up the drop down box for the flightmode
-    UAVDataObject *flightStatus = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
-    m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions());
+    // TODO: update this with named modes based on current configuration
+    m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 1");
+    m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 2");
+    m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 3");
+    m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 4");
+    m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 5");
+    m_gcscontrol->comboBoxFlightMode->addItem("Sw pos 6");
 
     // Set up slots and signals for joysticks
     connect(m_gcscontrol->widgetLeftStick, SIGNAL(positionClicked(double, double)), this, SLOT(leftStickClicked(double, double)));
@@ -62,7 +67,7 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
 
     // Connect misc controls
     connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int)));
-    connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
+    connect(m_gcscontrol->checkBoxArming, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
     connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
 
     connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)), this, SLOT(toggleUDPControl(int))); // UDP control checkbox
@@ -117,16 +122,17 @@ void GCSControlGadgetWidget::toggleControl(int state)
 {
     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
     UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
-    UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
+    UAVDataObject *obj  = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
+    UAVDataObject *obj2 = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("AccessoryDesired"), 0));
 
-    UAVObject::Metadata mdata    = obj->getMetadata();
+    UAVObject::Metadata mdata = obj->getMetadata();
 
     if (state) {
         mccInitialData = mdata;
         UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
-        UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
+        UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_MANUAL);
         UAVObject::SetGcsTelemetryAcked(mdata, false);
-        UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
+        UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
         mdata.gcsTelemetryUpdatePeriod = 100;
         m_gcscontrol->checkBoxUDPControl->setEnabled(true);
     } else {
@@ -135,18 +141,19 @@ void GCSControlGadgetWidget::toggleControl(int state)
         m_gcscontrol->checkBoxUDPControl->setEnabled(false);
     }
     obj->setMetadata(mdata);
+    obj2->setMetadata(mdata);
 }
 
 void GCSControlGadgetWidget::toggleArmed(int state)
 {
     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
     UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
-    UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
+    UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("AccessoryDesired"), 0));
 
     if (state) {
-        obj->getField("Armed")->setValue("Armed");
+        obj->getField("AccessoryVal")->setValue(1);
     } else {
-        obj->getField("Armed")->setValue("Disarmed");
+        obj->getField("AccessoryVal")->setValue(-1);
     }
     obj->updated();
 }
@@ -155,11 +162,7 @@ void GCSControlGadgetWidget::mccChanged(UAVObject *obj)
 {
     Q_UNUSED(obj);
 
-    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
-    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
-    UAVDataObject *flightStatus  = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
-    m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString()));
-    m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
+    m_gcscontrol->comboBoxFlightMode->setCurrentIndex(obj->getField("FlightModeSwitchPosition")->getValue().toInt());
 }
 
 void GCSControlGadgetWidget::toggleUDPControl(int state)
@@ -178,10 +181,9 @@ void GCSControlGadgetWidget::selectFlightMode(int state)
 {
     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
     UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
-    UAVDataObject *obj    = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("FlightStatus")));
-    UAVObjectField *field = obj->getField("FlightMode");
+    UAVDataObject *obj = dynamic_cast<UAVDataObject *>(objManager->getObject(QString("ManualControlCommand")));
 
-    field->setValue(field->getOptions()[state]);
+    obj->getField("FlightModeSwitchPosition")->setValue(state);
     obj->updated();
 }
 
@@ -204,6 +206,16 @@ bool GCSControlGadgetWidget::getUDPControl(void)
     return m_gcscontrol->checkBoxUDPControl->isChecked();
 }
 
+void GCSControlGadgetWidget::setArmed(bool newState)
+{
+    m_gcscontrol->checkBoxArming->setChecked(newState);
+}
+
+bool GCSControlGadgetWidget::getArmed(void)
+{
+    return m_gcscontrol->checkBoxArming->isChecked();
+}
+
 
 /**
  * @}
diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
index 817970299..79830a717 100644
--- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
+++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
@@ -45,6 +45,8 @@ public:
     bool getGCSControl(void);
     void setUDPControl(bool newState);
     bool getUDPControl(void);
+    void setArmed(bool newState);
+    bool getArmed(void);
 
 signals:
     void sticksChanged(double leftX, double leftY, double rightX, double rightY);