Mount: enable mount control via eeprom parameters

this enables MNT_* parameter control of the camera mount code. It also
fixes the conversion of calculated angles between degrees and
integers, and fixes stabilised mount control when yaw control is not
available.

ArduPlane/ArduPlane.pde

@@ -918,10 +918,6 @@ static void slow_loop()
 			update_servo_switches();
 
 			update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
-
-#if MOUNT == ENABLED
-			camera_mount.update_mount_type();
-#endif
 			break;
 
 		case 2:

ArduPlane/Parameters.h

@@ -124,9 +124,10 @@ public:
 
 
         //
-        // Camera parameters
+        // Camera and mount parameters
         //
-        k_param_camera,
+        k_param_camera = 159,
+        k_param_camera_mount,
 
         //
         // 170: Radio settings

ArduPlane/Parameters.pde

@@ -328,6 +328,13 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	// @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
 	GOBJECT(ahrs,					"AHRS_",    AP_AHRS),
 
+#if MOUNT == ENABLED
+	// @Group: MNT_
+	// @Path: ../libraries/AP_Mount/AP_Mount.cpp
+	GOBJECT(camera_mount,           "MNT_",	AP_Mount),
+#endif
+
+
 #ifdef DESKTOP_BUILD
 	// @Group: SIM_
     // @Path: ../libraries/SITL/SITL.cpp

libraries/AP_Mount/AP_Mount.cpp

@@ -1,7 +1,22 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Param.h>
 #include <AP_Mount.h>
 
+const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
+    // index 0 was used for the old orientation matrix
+    AP_GROUPINFO("MODE",       0, AP_Mount, _mount_mode),
+    AP_GROUPINFO("RETRACT",    1, AP_Mount, _retract_angles),
+    AP_GROUPINFO("NEUTRAL",    2, AP_Mount, _neutral_angles),
+    AP_GROUPINFO("CONTROL",    3, AP_Mount, _control_angles),
+    AP_GROUPINFO("STAB_ROLL",  4, AP_Mount, _stab_roll),
+    AP_GROUPINFO("STAB_PITCH", 5, AP_Mount, _stab_pitch),
+    AP_GROUPINFO("STAB_YAW",   6, AP_Mount, _stab_yaw),
+    AP_GROUPEND
+};
+
 extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions];	// the aux. servo ch. assigned to each function
 
 AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs):
@@ -10,37 +25,31 @@ _gps(gps)
 	_ahrs = ahrs;
 	_current_loc = current_loc;
 
-	//set_mode(MAV_MOUNT_MODE_RETRACT);
-	set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
-	//set_mode(MAV_MOUNT_MODE_GPS_POINT); // FIXME: this is to test ONLY targeting
+	// startup with the mount retracted
+	set_mode(MAV_MOUNT_MODE_RETRACT);
 
-	_retract_angles.x=0;
-	_retract_angles.y=0;
-	_retract_angles.z=0;
+	// default to zero angles
+	_retract_angles = Vector3f(0,0,0);
+	_neutral_angles = Vector3f(0,0,0);
+	_control_angles = Vector3f(0,0,0);
 }
 
-//sets the servo angles for retraction, note angles are * 100
-void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
+//sets the servo angles for retraction, note angles are in degrees
+void AP_Mount::set_retract_angles(float roll, float pitch, float yaw)
 {
-	_retract_angles.x=roll;
-	_retract_angles.y=pitch;
-	_retract_angles.z=yaw;
+	_retract_angles = Vector3f(roll, pitch, yaw);
 }
 
-//sets the servo angles for neutral, note angles are * 100
-void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw)
+//sets the servo angles for neutral, note angles are in degrees
+void AP_Mount::set_neutral_angles(float roll, float pitch, float yaw)
 {
-	_neutral_angles.x=roll;
-	_neutral_angles.y=pitch;
-	_neutral_angles.z=yaw;
+	_neutral_angles = Vector3f(roll, pitch, yaw);
 }
 
-//sets the servo angles for MAVLink, note angles are * 100
-void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw)
+//sets the servo angles for MAVLink, note angles are in degrees
+void AP_Mount::set_control_angles(float roll, float pitch, float yaw)
 {
-	_mavlink_angles.x = roll;
-	_mavlink_angles.y = pitch;
-	_mavlink_angles.z = yaw;
+	_control_angles = Vector3f(roll, pitch, yaw);
 }
 
 // used to tell the mount to track GPS location
@@ -52,28 +61,35 @@ void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
 // This one should be called periodically
 void AP_Mount::update_mount_position()
 {
-	switch(_mount_mode)
+	switch((enum MAV_MOUNT_MODE)_mount_mode.get())
 	{
 		// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
 		case MAV_MOUNT_MODE_RETRACT:
-			_roll_angle =100*_retract_angles.x;
-			_pitch_angle=100*_retract_angles.y;
-			_yaw_angle  =100*_retract_angles.z;
+		{
+			Vector3f vec = _retract_angles.get();
+			_roll_angle  = vec.x;
+			_pitch_angle = vec.y;
+			_yaw_angle   = vec.z;
 			break;
+		}
 
 		// move mount to a neutral position, typically pointing forward
 		case MAV_MOUNT_MODE_NEUTRAL:
-			_roll_angle =100*_neutral_angles.x;
-			_pitch_angle=100*_neutral_angles.y;
-			_yaw_angle  =100*_neutral_angles.z;
+		{
+			Vector3f vec = _neutral_angles.get();
+			_roll_angle  = vec.x;
+			_pitch_angle = vec.y;
+			_yaw_angle   = vec.z;
 			break;
+		}
 
 		// point to the angles given by a mavlink message
 		case MAV_MOUNT_MODE_MAVLINK_TARGETING:
 		{
-			_roll_control_angle  = _mavlink_angles.x;
-			_pitch_control_angle = _mavlink_angles.y;
-			_yaw_control_angle   = _mavlink_angles.z;
+			Vector3f vec = _control_angles.get();
+			_roll_control_angle  = vec.x;
+			_pitch_control_angle = vec.y;
+			_yaw_control_angle   = vec.z;
 			calculate();
 			break;
 		}
@@ -81,9 +97,10 @@ void AP_Mount::update_mount_position()
 		// RC radio manual angle control, but with stabilization from the AHRS
 		case MAV_MOUNT_MODE_RC_TARGETING:
 		{
-			G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle);
-			G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle);
-			G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle);
+			// rc_input() takes degrees * 100 units
+			G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100);
+			G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100);
+			G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100);
 			if (_ahrs){
 				calculate();
 			} else {
@@ -117,15 +134,15 @@ void AP_Mount::update_mount_position()
 	G_RC_AUX(k_mount_pitch)->angle_out(_pitch_angle);
 	G_RC_AUX(k_mount_yaw)->angle_out(_yaw_angle);
 */
-	// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
-	G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle/10);
-	G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle/10);
-	G_RC_AUX(k_mount_yaw)->closest_limit(_yaw_angle/10);
+	// closest_limit() takes degrees * 10 units
+	G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle*10);
+	G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle*10);
+	G_RC_AUX(k_mount_yaw)->closest_limit(_yaw_angle*10);
 }
 
 void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
 {
-	_mount_mode=mode;
+	_mount_mode = (int8_t)mode;
 }
 
 // Change the configuration of the mount
@@ -155,26 +172,26 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
 		return;
 	}
 
-	switch (_mount_mode)
+	switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
 	{
 	case MAV_MOUNT_MODE_RETRACT:  // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
-		set_retract_angles(packet.input_b, packet.input_a, packet.input_c);
+		set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
 		if (packet.save_position)
 		{
-			// TODO: Save current trimmed position on EEPROM
+			_retract_angles.save();
 		}
 		break;
 
 	case MAV_MOUNT_MODE_NEUTRAL:  //  Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
-		set_neutral_angles(packet.input_b, packet.input_a, packet.input_c);
+		set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
 		if (packet.save_position)
 		{
-			// TODO: Save current trimmed position on EEPROM
+			_neutral_angles.save();
 		}
 		break;
 
 	case MAV_MOUNT_MODE_MAVLINK_TARGETING:  // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
-		set_mavlink_angles(packet.input_b, packet.input_a, packet.input_c);
+		set_control_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
 		break;
 
 	case MAV_MOUNT_MODE_RC_TARGETING:  // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
@@ -187,6 +204,9 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
 		targetGPSLocation.alt = packet.input_c;
 		set_GPS_target_location(targetGPSLocation);
 		break;
+
+	case MAV_MOUNT_MODE_ENUM_END:
+		break;
 	}
 }
 
@@ -201,21 +221,23 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
 		return;
 	}
 
-	switch (_mount_mode)
+	switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
 	{
 	case MAV_MOUNT_MODE_RETRACT:			// safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
 	case MAV_MOUNT_MODE_NEUTRAL:			// neutral position (Roll,Pitch,Yaw) from EEPROM
 	case MAV_MOUNT_MODE_MAVLINK_TARGETING:	// neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
 	case MAV_MOUNT_MODE_RC_TARGETING:		// neutral position and start RC Roll,Pitch,Yaw control with stabilization
-		packet.pointing_b = _roll_angle;	///< degrees*100
-		packet.pointing_a = _pitch_angle;	///< degrees*100
-		packet.pointing_c = _yaw_angle;		///< degrees*100
+		packet.pointing_b = _roll_angle*100;	///< degrees*100
+		packet.pointing_a = _pitch_angle*100;	///< degrees*100
+		packet.pointing_c = _yaw_angle*100;		///< degrees*100
 		break;
 	case MAV_MOUNT_MODE_GPS_POINT:         // neutral position and start to point to Lat,Lon,Alt
 		packet.pointing_a = _target_GPS_location.lat;	///< latitude
 		packet.pointing_b = _target_GPS_location.lng;	///< longitude
 		packet.pointing_c = _target_GPS_location.alt;	///< altitude
 		break;
+	case MAV_MOUNT_MODE_ENUM_END:
+		break;
 	}
 
 	// status reply
@@ -258,44 +280,35 @@ AP_Mount::calc_GPS_target_angle(struct Location *target)
 	}
 }
 
-// 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;
-	}
-}
-
 // Inputs desired _roll_control_angle, _pitch_control_angle and _yaw_control_angle stabilizes them relative to the airframe
 // and calculates output _roll_angle, _pitch_angle and _yaw_angle
 void
 AP_Mount::calculate()
 {
-	Matrix3f m;             ///< holds 3 x 3 matrix, var is used as temp in calcs
-	Matrix3f cam;           ///< Rotation matrix earth to camera. Desired camera from input.
-	Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
-	float roll;
-	float pitch;
-	float yaw;
-	if (_ahrs){
-		m = _ahrs->get_dcm_matrix();
-		m.transpose();
-		cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
-		gimbal_target = m * cam;
-		gimbal_target.to_euler(&roll, &pitch, &yaw);
-		_roll_angle  = degrees(roll)*100;
-		_pitch_angle = degrees(pitch)*100;
-		_yaw_angle   = degrees(yaw)*100;
+	if (_ahrs) {
+		// only do the full 3D frame transform if we are doing yaw control
+		if (_stab_yaw) {
+			Matrix3f m;             ///< holds 3 x 3 matrix, var is used as temp in calcs
+			Matrix3f cam;           ///< Rotation matrix earth to camera. Desired camera from input.
+			Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
+			m = _ahrs->get_dcm_matrix();
+			m.transpose();
+			cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
+			gimbal_target = m * cam;
+			gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle);
+		} else {
+			// otherwise base mount roll and pitch on the ahrs
+			// roll/pitch attitude, plus any requested angle
+			_roll_angle  = _roll_control_angle;
+			_pitch_angle = _pitch_control_angle;
+			_yaw_angle   = _yaw_control_angle;
+			if (_stab_roll) {
+				_roll_angle -= degrees(_ahrs->roll);
+			}
+			if (_stab_pitch) {
+				_pitch_angle -= degrees(_ahrs->pitch);
+			}
+		}
 	}
 }
 

libraries/AP_Mount/AP_Mount.h

@@ -52,19 +52,20 @@ public:
 
 	// 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();
+
+	// hook for eeprom variables
+    static const struct AP_Param::GroupInfo var_info[];
 
 private:
 
 	//methods
 	void set_mode(enum MAV_MOUNT_MODE mode);
 
-	void set_retract_angles(int roll, int pitch, int yaw);		///< set mount retracted position
-	void set_neutral_angles(int roll, int pitch, int yaw);
-	void set_mavlink_angles(int roll, int pitch, int yaw);
+	void set_retract_angles(float roll, float pitch, float yaw);		///< set mount retracted position
+	void set_neutral_angles(float roll, float pitch, float yaw);
+	void set_control_angles(float roll, float pitch, float yaw);
 	void set_GPS_target_location(Location targetGPSLocation);	///< used to tell the mount to track GPS location
 
 	// internal methods
@@ -81,21 +82,20 @@ private:
 	float _pitch_control_angle;
 	float _yaw_control_angle;
 
-	int16_t _roll_angle;	 ///< degrees*100
-	int16_t _pitch_angle;	 ///< degrees*100
-	int16_t _yaw_angle;		 ///< degrees*100
+	float _roll_angle;	 ///< degrees
+	float _pitch_angle;	 ///< degrees
+	float _yaw_angle;	 ///< degrees
 
-	uint8_t _stab_roll;  ///< (1 = yes, 0 = no)
-	uint8_t _stab_pitch; ///< (1 = yes, 0 = no)
-	uint8_t _stab_yaw;   ///< (1 = yes, 0 = no)
+	AP_Int8 _stab_roll;  ///< (1 = yes, 0 = no)
+	AP_Int8 _stab_pitch; ///< (1 = yes, 0 = no)
+	AP_Int8 _stab_yaw;   ///< (1 = yes, 0 = no)
 
-	enum MAV_MOUNT_MODE _mount_mode;
-	MountType _mount_type;
+	AP_Int8 _mount_mode;
 
 	struct Location _target_GPS_location;
 
-	Vector3i _retract_angles;		///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
-	Vector3i _neutral_angles;		///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
-	Vector3i _mavlink_angles;		///< mavlink position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
+	AP_Vector3f _retract_angles;		///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
+	AP_Vector3f _neutral_angles;		///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
+	AP_Vector3f _control_angles;		///< GCS controlled position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
 };
 #endif