AP_Mount: addition of Amilcar's mount_type

set_roi_cmd modified to take a Location.  I know the intention was for set_roi_cmd to interpret the MAVLink command directly but unfortunately in arduCopter missions are only made up of waypoints (which are Locations).

libraries/AP_Mount/AP_Mount.cpp

@@ -79,11 +79,31 @@ _gps(gps)
 	_neutral_angles = Vector3f(0,0,0);
 	_control_angles = Vector3f(0,0,0);
 
+	// default mount type to roll/pitch (which is the most common for copter)
+	_mount_type = k_tilt_roll;
+
 	// default manual rc channel to disabled
 	_manual_rc = NULL;
 	_manual_rc_function = AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED;
 }
 
+// Auto-detect the mount gimbal type depending on the functions assigned to the servos
+void AP_Mount::update_mount_type()
+{
+	if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
+	{
+		_mount_type = k_pan_tilt;
+	}
+	if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL))
+	{
+		_mount_type = k_tilt_roll;
+	}
+	if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
+	{
+		_mount_type = k_pan_tilt_roll;
+	}
+}
+
 //sets the servo angles for retraction, note angles are in degrees
 void AP_Mount::set_retract_angles(float roll, float pitch, float yaw)
 {
@@ -279,6 +299,10 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
 
 	case MAV_MOUNT_MODE_ENUM_END:
 		break;
+
+	default:
+		// do nothing
+		break;
 	}
 }
 
@@ -319,9 +343,13 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
 }
 
 // Set mount point/region of interest, triggered by mission script commands
-void AP_Mount::set_roi_cmd()
+void AP_Mount::set_roi_cmd(struct Location *target_loc)
 {
-	// TODO get the information out of the mission command and use it
+	// set the target gps location
+	_target_GPS_location = *target_loc;
+
+	// set the mode to GPS tracking mode
+	set_mode(MAV_MOUNT_MODE_GPS_POINT);
 }
 
 // Set mount configuration, triggered by mission script commands

libraries/AP_Mount/AP_Mount.h

@@ -41,17 +41,27 @@ public:
 	//Constructor
 	AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs);
 
+	//enums
+	enum MountType{
+		k_pan_tilt = 0,			///< yaw-pitch
+		k_tilt_roll = 1,		///< pitch-roll
+		k_pan_tilt_roll = 2,	///< yaw-pitch-roll
+	};
+
 	// MAVLink methods
 	void configure_msg(mavlink_message_t* msg);
 	void control_msg(mavlink_message_t* msg);
 	void status_msg(mavlink_message_t* msg);
-	void set_roi_cmd();
+	void set_roi_cmd(struct Location *target_loc);
 	void configure_cmd();
 	void control_cmd();
 
 	// should be called periodically
 	void update_mount_position();
+	void update_mount_type();		///< Auto-detect the mount gimbal type depending on the functions assigned to the servos
 	void debug_output();			///< For testing and development. Called in the medium loop.
+	// Accessors
+	enum MountType get_mount_type() { return _mount_type; }
 
 	// to allow manual input of an angle from the pilot when RC_Channel_aux cannot be used
 	void set_manual_rc_channel(RC_Channel *rc);			// define which RC_Channel is to be used for manual control
@@ -92,6 +102,7 @@ private:
 	AP_Int8 _stab_yaw;   ///< (1 = yes, 0 = no)
 
 	AP_Int8 _mount_mode;
+	MountType _mount_type;
 
 	struct Location _target_GPS_location;