ArduCopter: add support for MAVLink's MAV_CMD_NAV_ROI command

do_nav_roi and verify_nav_roi functions added to turn the copter and/or mount in response to a MAV_CMD_NAV_ROI way point

ArduCopter/GCS_Mavlink.pde

@@ -1190,7 +1190,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
 			}
 
-			// Switch to map APM command fields inot MAVLink command fields
+			// Switch to map APM command fields into MAVLink command fields
 			switch (tell_command.id) {
 
 				case MAV_CMD_NAV_LOITER_TURNS:
@@ -1199,6 +1199,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					param1 = tell_command.p1;
 					break;
 
+				case MAV_CMD_NAV_ROI:
+					param1 = tell_command.p1;	// MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters
+					break;
+
 				case MAV_CMD_CONDITION_YAW:
 					param3 = tell_command.p1;
 					param1 = tell_command.alt;
@@ -1415,15 +1419,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
 			tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
 			tell_command.alt = packet.z * 1.0e2;
-			tell_command.options = 1; // store altitude relative!! Always!!
+			tell_command.options = 1; // store altitude relative to home alt!! Always!!
 
-			switch (tell_command.id) {					// Switch to map APM command fields inot MAVLink command fields
+			switch (tell_command.id) {					// Switch to map APM command fields into MAVLink command fields
 				case MAV_CMD_NAV_LOITER_TURNS:
 				case MAV_CMD_DO_SET_HOME:
-				case MAV_CMD_DO_SET_ROI:
 					tell_command.p1 = packet.param1;
 					break;
 
+				case MAV_CMD_NAV_ROI:
+					tell_command.p1 = packet.param1;	// MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters
+					break;
+
 				case MAV_CMD_CONDITION_YAW:
 					tell_command.p1 = packet.param3;
 					tell_command.alt = packet.param1;
@@ -1761,6 +1768,40 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         }
 #endif // HIL_MODE
 
+#if CAMERA == ENABLED
+    case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
+		{
+			g.camera.configure_msg(msg);
+			break;
+		}
+
+    case MAVLINK_MSG_ID_DIGICAM_CONTROL:
+		{
+			g.camera.control_msg(msg);
+			break;
+		}
+#endif // CAMERA == ENABLED
+
+#if MOUNT == ENABLED
+    case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
+		{
+			camera_mount.configure_msg(msg);
+			break;
+		}
+
+    case MAVLINK_MSG_ID_MOUNT_CONTROL:
+		{
+			camera_mount.control_msg(msg);
+			break;
+		}
+
+    case MAVLINK_MSG_ID_MOUNT_STATUS:
+		{
+			camera_mount.status_msg(msg);
+			break;
+		}
+#endif // MOUNT == ENABLED
+
     case MAVLINK_MSG_ID_RADIO:
         {
             mavlink_radio_t packet;

ArduCopter/commands_logic.pde

@@ -36,6 +36,11 @@ static void process_nav_command()
 			do_RTL();
 			break;
 
+		// point the copter and camera at a region of interest (ROI)
+		case MAV_CMD_NAV_ROI: // 80
+			do_nav_roi();
+			break;
+
 		default:
 			break;
 	}
@@ -110,17 +115,7 @@ static void process_now_command()
 			break;
 #endif
 
-		// Sets the region of interest (ROI) for a sensor set or the
-		// vehicle itself. This can then be used by the vehicles control
-		// system to control the vehicle attitude and the attitude of various
-		// devices such as cameras.
-		//    |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
-		case MAV_CMD_DO_SET_ROI: // 201
-			do_target_yaw();
 #if MOUNT == ENABLED
-			camera_mount.set_roi_cmd();
-			break;
-
 		case MAV_CMD_DO_MOUNT_CONFIGURE:	// Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
 			camera_mount.configure_cmd();
 			break;
@@ -129,6 +124,10 @@ static void process_now_command()
 			camera_mount.control_cmd();
 			break;
 #endif
+
+		default:
+			// do nothing with unrecognized MAVLink messages
+			break;
 	}
 }
 
@@ -144,6 +143,10 @@ static bool verify_must()
 			return verify_takeoff();
 			break;
 
+		case MAV_CMD_NAV_WAYPOINT:
+			return verify_nav_wp();
+			break;
+
 		case MAV_CMD_NAV_LAND:
 			if(g.sonar_enabled == true){
 				return verify_land_sonar();
@@ -152,10 +155,6 @@ static bool verify_must()
 			}
 			break;
 
-		case MAV_CMD_NAV_WAYPOINT:
-			return verify_nav_wp();
-			break;
-
 		case MAV_CMD_NAV_LOITER_UNLIM:
 			return verify_loiter_unlimited();
 			break;
@@ -172,6 +171,10 @@ static bool verify_must()
 			return verify_RTL();
 			break;
 
+		case MAV_CMD_NAV_ROI: // 80
+			return verify_nav_roi();
+			break;
+
 		default:
 			//gcs_send_text_P(SEVERITY_HIGH,PSTR("<verify_must: default> No current Must commands"));
 			return false;
@@ -695,6 +698,39 @@ static bool verify_yaw()
 	}
 }
 
+// verify_nav_roi - verifies that actions required by MAV_CMD_NAV_ROI have completed
+//              we assume the camera command has been successfully implemented by the do_nav_roi command
+//              so all we need to check is whether we needed to yaw the copter (due to the mount type) and
+//              whether that yaw has completed
+//	TO-DO: add support for other features of MAV_NAV_ROI including pointing at a given waypoint
+static bool verify_nav_roi()
+{
+#if MOUNT == ENABLED
+	// check if mount type requires us to rotate the quad
+	if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) {
+		// ensure yaw has gotten to within 2 degrees of the target
+		if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) {
+			nav_yaw = auto_yaw;	// ensure target yaw for YAW_HOLD is our desired yaw
+			return true;
+		}else{
+			return false;
+		}
+	}else{
+		// if no rotation required, assume the camera instruction was implemented immediately
+		return true;
+	}
+#else
+	// if we have no camera mount simply check we've reached the desired yaw
+	// ensure yaw has gotten to within 2 degrees of the target
+	if( abs(wrap_180(ahrs.yaw_sensor-auto_yaw)) <= 200 ) {
+		nav_yaw = auto_yaw;	// ensure target yaw for YAW_HOLD is our desired yaw
+		return true;
+	}else{
+		return false;
+	}
+#endif
+}
+
 /********************************************************************************/
 //	Do (Now) commands
 /********************************************************************************/
@@ -817,3 +853,35 @@ static void do_repeat_relay()
 	event_repeat	= command_cond_queue.alt * 2;
 	update_events();
 }
+
+// do_nav_roi - starts actions required by MAV_CMD_NAV_ROI
+//              this involves either moving the camera to point at the ROI (region of interest)
+//              and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
+//				Note: the ROI should already be in the command_nav_queue global variable
+//	TO-DO: add support for other features of MAV_NAV_ROI including pointing at a given waypoint
+static void do_nav_roi()
+{
+#if MOUNT == ENABLED
+
+	// check if mount type requires us to rotate the quad
+	if( camera_mount.get_mount_type() == AP_Mount::k_tilt_roll ) {
+		yaw_tracking = MAV_ROI_LOCATION;
+		target_WP = command_nav_queue;
+		auto_yaw = get_bearing(&current_loc, &target_WP);
+	}
+	// send the command to the camera mount
+	camera_mount.set_roi_cmd(&command_nav_queue);
+
+	// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink.  Currently we only handle mode 4 (see below)
+	//		0: do nothing
+	//		1: point at next waypoint
+	//		2: point at a waypoint taken from WP# parameter (2nd parameter?)
+	//		3: point at a location given by alt, lon, lat parameters
+	//		4: point at a target given a target id (can't be implmented)
+#else
+	// if we have no camera mount simply rotate the quad
+	yaw_tracking = MAV_ROI_LOCATION;
+	target_WP = command_nav_queue;
+	auto_yaw = get_bearing(&current_loc, &target_WP);
+#endif
+}