diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 08a89ca3fb62c9aa7098a29ad90ca23bc2edff15..6935c5fbcf158feb4e84a504b7a5d25b068a3753 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -530,14 +530,22 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         // unused
         break;
 
-    case MSG_RETRY_DEFERRED:
-    case MSG_TERRAIN:
-        break; // just here to prevent a warning
-
     case MSG_BATTERY2:
         CHECK_PAYLOAD_SIZE(BATTERY2);
         gcs[chan-MAVLINK_COMM_0].send_battery2(battery);
         break;
+
+    case MSG_CAMERA_FEEDBACK:
+#if CAMERA == ENABLED
+        CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
+        camera.send_feedback(chan, gps, ahrs, current_loc);
+#endif
+        break;
+
+    case MSG_RETRY_DEFERRED:
+    case MSG_TERRAIN:
+    case MSG_OPTICAL_FLOW:
+        break; // just here to prevent a warning
 	}
 
     
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index 3ab28ec3970f2c0f7c3cb5c6d1c0c2debf38627f..69fcfaff4e93684e046e374ac91b7a69642bce4b 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -308,6 +308,7 @@ static void do_take_picture()
 {
 #if CAMERA == ENABLED
     camera.trigger_pic();
+    gcs_send_message(MSG_CAMERA_FEEDBACK);
     if (should_log(MASK_LOG_CAMERA)) {
         DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
     }
diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde
index 7bc62c66422d7644351c62ca3fc65d164a11a66e..08e49ae2239423ce45ff7656d61bcdf35ccb4399 100644
--- a/APMrover2/setup.pde
+++ b/APMrover2/setup.pde
@@ -341,9 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 				mode != AUTO &&
 				mode != RTL) 
 			{
-				if (mode < MANUAL)
-					mode = RTL;
-				else if (mode > RTL)
+				if (mode > RTL)
 					mode = MANUAL;
 				else
 					mode += radioInputSwitch;