diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
index a3a76850db6c21c1ebe72a9aa99fea3f933492cf..c08a0fa969139c2b5ae8abc61a76a14316798a78 100644
--- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
@@ -35,7 +35,13 @@ APMSubMode::APMSubMode(uint32_t mode, bool settable) :
     QMap<uint32_t,QString> enumToString;
     enumToString.insert(MANUAL, "Manual");
     enumToString.insert(STABILIZE, "Stabilize");
+    enumToString.insert(ACRO, "Acro");
     enumToString.insert(ALT_HOLD,  "Depth Hold");
+    enumToString.insert(AUTO, "Auto");
+    enumToString.insert(GUIDED, "Guided");
+    enumToString.insert(CIRCLE, "Circle");
+    enumToString.insert(SURFACE, "Surface");
+    enumToString.insert(POSHOLD, "Position Hold");
 
     setEnumToStringMapping(enumToString);
 }
@@ -46,7 +52,13 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void):
     QList<APMCustomMode> supportedFlightModes;
     supportedFlightModes << APMSubMode(APMSubMode::MANUAL ,true);
     supportedFlightModes << APMSubMode(APMSubMode::STABILIZE ,true);
+    supportedFlightModes << APMSubMode(APMSubMode::ACRO ,true);
     supportedFlightModes << APMSubMode(APMSubMode::ALT_HOLD  ,true);
+    supportedFlightModes << APMSubMode(APMSubMode::AUTO ,true);
+    supportedFlightModes << APMSubMode(APMSubMode::GUIDED ,true);
+    supportedFlightModes << APMSubMode(APMSubMode::CIRCLE ,true);
+    supportedFlightModes << APMSubMode(APMSubMode::SURFACE ,false);
+    supportedFlightModes << APMSubMode(APMSubMode::POSHOLD ,true);
     setSupportedModes(supportedFlightModes);
 
     if (!_remapParamNameIntialized) {
diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
index 2b0c7aa04c986ca8797d11aa849f4d239e1826ad..8c050028e1a3717f76ed142cd256f338e7807ea3 100644
--- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
@@ -68,22 +68,22 @@ class APMSubMode : public APMCustomMode
 public:
     enum Mode {
         STABILIZE         = 0,   // Hold level position
-        RESERVED_1        = 1,
+        ACRO              = 1,   // Manual angular rate, throttle
         ALT_HOLD          = 2,   // Depth hold
-        RESERVED_3        = 3,
-        RESERVED_4        = 4,
+        AUTO              = 3,   // Full auto to waypoint
+        GUIDED            = 4,   // Full auto to coordinate/direction
         RESERVED_5        = 5,
         RESERVED_6        = 6,
-        RESERVED_7        = 7,
+        CIRCLE            = 7,   // Auto circling
         RESERVED_8        = 8,
-        RESERVED_9        = 9,
+        SURFACE           = 9,   // Auto return to surface
         RESERVED_10       = 10,
         RESERVED_11       = 11,
         RESERVED_12       = 12,
         RESERVED_13       = 13,
         RESERVED_14       = 14,
         RESERVED_15       = 15,
-        RESERVED_16       = 16,
+        POSHOLD           = 16,  // Hold position
         RESERVED_17       = 17,
         RESERVED_18       = 18,
         MANUAL            = 19