AP_Mount: adapt library for AHRS framework

libraries/AP_Mount/AP_Mount.cpp

@@ -4,10 +4,9 @@
 
 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(GPS *gps, AP_DCM *dcm)
+AP_Mount::AP_Mount(GPS *gps, AP_AHRS *ahrs)
 {
-	_dcm=dcm;
-	_dcm_hil=NULL;
+	_ahrs=ahrs;
 	_gps=gps;
 	//set_mode(MAV_MOUNT_MODE_RETRACT);
 	//set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
@@ -18,20 +17,6 @@ AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
 	_retract_angles.z=0;
 }
 
-AP_Mount::AP_Mount(GPS *gps, AP_DCM_HIL *dcm)
-{
-	_dcm=NULL;
-	_dcm_hil=dcm;
-	_gps=gps;
-	//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_MAVLINK_TARGETING); // FIXME: this is to test ONLY targeting
-
-	_retract_angles.x=0;
-	_retract_angles.y=0;
-	_retract_angles.z=0;
-}
-
 //sets the servo angles for retraction, note angles are * 100
 void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
 {
@@ -88,7 +73,8 @@ void AP_Mount::update_mount_position()
 		aux_vec.x = _mavlink_angles.x;
 		aux_vec.y = _mavlink_angles.y;
 		aux_vec.z = _mavlink_angles.z;
-		m = _dcm?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
+		m = _ahrs->get_dcm_matrix();
+		m.transpose();
 		//rotate vector
 		targ = m*aux_vec;
 		// TODO The next three lines are probably not correct yet
@@ -101,17 +87,11 @@ void AP_Mount::update_mount_position()
 	case MAV_MOUNT_MODE_RC_TARGETING:  // radio manual control
 	{
 		// TODO It does work, but maybe is a good idea to replace this simplified implementation with a proper one
-		if (_dcm)
-		{
-			roll_angle  = -_dcm->roll_sensor;
-			pitch_angle = -_dcm->pitch_sensor;
-			yaw_angle   = -_dcm->yaw_sensor;
-		}
-		if (_dcm_hil)
+		if (_ahrs)
 		{
-			roll_angle  = -_dcm_hil->roll_sensor;
-			pitch_angle = -_dcm_hil->pitch_sensor;
-			yaw_angle   = -_dcm_hil->yaw_sensor;
+			roll_angle  = -_ahrs->roll_sensor;
+			pitch_angle = -_ahrs->pitch_sensor;
+			yaw_angle   = -_ahrs->yaw_sensor;
 		}
 		if (g_rc_function[RC_Channel_aux::k_mount_roll])
 			roll_angle  = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
@@ -128,7 +108,8 @@ void AP_Mount::update_mount_position()
 		{
 			calc_GPS_target_vector(&_target_GPS_location);
 		}
-		m = (_dcm)?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
+		m = _ahrs->get_dcm_matrix();
+		m.transpose();
 		targ = m*_GPS_vector;
 		/* disable stabilization for now, this will help debug */
 		_stab_roll = 0;_stab_pitch=0;_stab_yaw=0;

libraries/AP_Mount/AP_Mount.h

@@ -24,7 +24,7 @@
 #include <AP_Math.h>
 #include <AP_Common.h>
 #include <AP_GPS.h>
-#include <AP_DCM.h>
+#include <AP_AHRS.h>
 #include <GCS_MAVLink.h>
 #include <../RC_Channel/RC_Channel_aux.h>
 
@@ -32,8 +32,7 @@ class AP_Mount
 {
 public:
 	//Constructors
-	AP_Mount(GPS *gps, AP_DCM *dcm);
-	AP_Mount(GPS *gps, AP_DCM_HIL *dcm); // constructor for HIL usage
+	AP_Mount(GPS *gps, AP_AHRS *ahrs);
 
 	//enums
 	enum MountType{
@@ -72,8 +71,7 @@ private:
 	long rc_map(RC_Channel_aux* rc_ch);
 
 	//members
-	AP_DCM		*_dcm;
-	AP_DCM_HIL  *_dcm_hil;
+	AP_AHRS		*_ahrs;
 	GPS 		*_gps;
 
 	int roll_angle;		///< degrees*100