AHRS: adapt ArduCopter for new AHRS framework

ArduCopter/ArduCopter.pde

@@ -73,8 +73,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
 #include <AP_PeriodicProcess.h>         // Parent header of Timer
                                         // (only included for makefile libpath to work)
 #include <AP_TimerProcess.h>            // TimerProcess is the scheduler for MPU6000 reads.
-#include <AP_Quaternion.h>  // Madgwick quaternion system
-#include <AP_DCM.h>         // ArduPilot Mega DCM Library
+#include <AP_AHRS.h>
 #include <APM_PI.h>            	// PI library
 #include <AC_PID.h>            // PID library
 #include <RC_Channel.h>     // RC Channel Library
@@ -235,12 +234,9 @@ AP_IMU_INS  imu(&ins);
 static GPS         *g_gps_null;
 
 #if QUATERNION_ENABLE == ENABLED
-  // this shouldn't be called dcm of course, but until we
-  // decide to actually use something else, I don't want the patch
-  // size to be huge
- AP_Quaternion dcm(&imu, g_gps_null);
+ AP_AHRS_Quaternion ahrs(&imu, g_gps_null);
 #else
- AP_DCM  dcm(&imu, g_gps_null);
+ AP_AHRS_DCM ahrs(&imu, g_gps_null);
 #endif
 
 AP_TimerProcess timer_scheduler;
@@ -251,7 +247,7 @@ AP_TimerProcess timer_scheduler;
 	AP_Compass_HIL          compass;
 	AP_GPS_HIL              g_gps_driver(NULL);
     AP_IMU_Shim imu;
-    AP_DCM  dcm(&imu, g_gps);
+    AP_AHRS_DCM  ahrs(&imu, g_gps);
     AP_PeriodicProcessStub timer_scheduler;
     AP_InertialSensor_Stub ins;
 
@@ -259,11 +255,11 @@ AP_TimerProcess timer_scheduler;
 
 #elif HIL_MODE == HIL_MODE_ATTITUDE
 	AP_ADC_HIL              adc;
-	AP_DCM_HIL              dcm;
+	AP_IMU_Shim             imu; // never used
+    AP_AHRS_HIL             ahrs(&imu, g_gps);
 	AP_GPS_HIL              g_gps_driver(NULL);
 	AP_Compass_HIL          compass; // never used
     AP_Baro_BMP085_HIL      barometer;
-	AP_IMU_Shim             imu; // never used
     AP_InertialSensor_Stub ins;
     AP_PeriodicProcessStub timer_scheduler;
 	#ifdef OPTFLOW_ENABLED
@@ -984,7 +980,7 @@ static void medium_loop()
 				if(g.compass_enabled){
 					if (compass.read()) {
                         // Calculate heading
-                        Matrix3f m = dcm.get_dcm_matrix();
+                        Matrix3f m = ahrs.get_dcm_matrix();
                         compass.calculate(m);
                         compass.null_offsets(m);
                     }
@@ -1297,7 +1293,7 @@ static void update_optical_flow(void)
     static int log_counter = 0;
 
 	optflow.update();
-	optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt);  // updates internal lon and lat with estimation based on optical flow
+	optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt);  // updates internal lon and lat with estimation based on optical flow
 
 	// write to log
 	log_counter++;
@@ -1555,7 +1551,7 @@ void update_simple_mode(void)
 	// which improves speed of function
 	simple_counter++;
 
-	int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
+	int delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100;
 
 	if (simple_counter == 1){
 		// roll
@@ -1863,17 +1859,17 @@ static void read_AHRS(void)
 	// Perform IMU calculations and get attitude info
 	//-----------------------------------------------
 	#if HIL_MODE != HIL_MODE_DISABLED
-		// update hil before dcm update
+		// update hil before ahrs update
 		gcs_update();
 	#endif
 
-	dcm.update_DCM();
+	ahrs.update();
 	omega = imu.get_gyro();
 }
 
 static void update_trig(void){
 	Vector2f yawvector;
-	Matrix3f temp 	= dcm.get_dcm_matrix();
+	Matrix3f temp 	= ahrs.get_dcm_matrix();
 
 	yawvector.x 	= temp.a.x; // sin
 	yawvector.y 	= temp.b.x;	// cos

ArduCopter/Attitude.pde

@@ -4,7 +4,7 @@ static int16_t
 get_stabilize_roll(int32_t target_angle)
 {
 	// angle error
-	target_angle 		= wrap_180(target_angle - dcm.roll_sensor);
+	target_angle 		= wrap_180(target_angle - ahrs.roll_sensor);
 
 #if FRAME_CONFIG == HELI_FRAME
 
@@ -33,7 +33,7 @@ static int16_t
 get_stabilize_pitch(int32_t target_angle)
 {
 	// angle error
-	target_angle 		= wrap_180(target_angle - dcm.pitch_sensor);
+	target_angle 		= wrap_180(target_angle - ahrs.pitch_sensor);
 
 #if FRAME_CONFIG == HELI_FRAME
 	// limit the error we're feeding to the PID
@@ -60,7 +60,7 @@ static int16_t
 get_stabilize_yaw(int32_t target_angle)
 {
 	// angle error
-	target_angle 		= wrap_180(target_angle - dcm.yaw_sensor);
+	target_angle 		= wrap_180(target_angle - ahrs.yaw_sensor);
 
 #if FRAME_CONFIG == HELI_FRAME  // cannot use rate control for helicopters
 	// limit the error we're feeding to the PID
@@ -320,13 +320,13 @@ get_nav_yaw_offset(int yaw_input, int reset)
 
 	if(reset == 0){
 		// we are on the ground
-		return dcm.yaw_sensor;
+		return ahrs.yaw_sensor;
 
 	}else{
 		// re-define nav_yaw if we have stick input
 		if(yaw_input != 0){
 			// set nav_yaw + or - the current location
-			_yaw = yaw_input + dcm.yaw_sensor;
+			_yaw = yaw_input + ahrs.yaw_sensor;
 			// we need to wrap our value so we can be 0 to 360 (*100)
 			return wrap_360(_yaw);
 
@@ -364,7 +364,7 @@ static int get_z_damping()
 
 float get_world_Z_accel()
 {
-	accels_rot = dcm.get_dcm_matrix() * imu.get_accel();
+	accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
 	//Serial.printf("z %1.4f\n", accels_rot.z);
 	return accels_rot.z;
 }
@@ -412,7 +412,7 @@ static float fullDampP = 0.100;
 
 float get_world_Z_accel()
 {
-	accels_rot = dcm.get_dcm_matrix() * imu.get_accel();
+	accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
 	return accels_rot.z;
 }
 

ArduCopter/Camera.pde

@@ -23,10 +23,10 @@ camera_stabilization()
 	// Allow user to control camera pitch with channel 6 (mixed with pitch DCM)
 	if(g.radio_tuning == 0) {
 		g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6));
-		g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor);
+		g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(ahrs.pitch_sensor);
 	}else{
 		// unless channel 6 is already being used for tuning
-	    g.rc_camera_pitch.servo_out = dcm.pitch_sensor  * -1;
+	    g.rc_camera_pitch.servo_out = ahrs.pitch_sensor  * -1;
 	}
 	g.rc_camera_pitch.servo_out	= (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain;
 
@@ -39,14 +39,14 @@ camera_stabilization()
 	// allow control mixing
 	/*
 	g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
-	g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-dcm.roll_sensor);
+	g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-ahrs.roll_sensor);
 	*/
 
 	// dont allow control mixing
-	g.rc_camera_roll.servo_out	= (float)-dcm.roll_sensor * g.camera_roll_gain;
+	g.rc_camera_roll.servo_out	= (float)-ahrs.roll_sensor * g.camera_roll_gain;
 
 	// limit
-	//g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500);
+	//g.rc_camera_roll.servo_out = constrain(-ahrs.roll_sensor, -4500, 4500);
 
 	// Output
 	// ------

ArduCopter/GCS_Mavlink.pde

@@ -38,9 +38,9 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
     mavlink_msg_attitude_send(
         chan,
         micros(),
-        dcm.roll,
-        dcm.pitch,
-        dcm.yaw,
+        ahrs.roll,
+        ahrs.pitch,
+        ahrs.yaw,
         omega.x,
         omega.y,
         omega.z);
@@ -99,7 +99,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
 
 static void NOINLINE send_location(mavlink_channel_t chan)
 {
-    Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
+    Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
     mavlink_msg_global_position_int_send(
         chan,
         current_loc.lat,
@@ -125,18 +125,18 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 }
 
 #if HIL_MODE != HIL_MODE_ATTITUDE
-static void NOINLINE send_dcm(mavlink_channel_t chan)
+static void NOINLINE send_ahrs(mavlink_channel_t chan)
 {
-    Vector3f omega_I = dcm.get_gyro_drift();
-    mavlink_msg_dcm_send(
+    Vector3f omega_I = ahrs.get_gyro_drift();
+    mavlink_msg_ahrs_send(
         chan,
         omega_I.x,
         omega_I.y,
         omega_I.z,
-        dcm.get_accel_weight(),
-        dcm.get_renorm_val(),
-        dcm.get_error_rp(),
-        dcm.get_error_yaw());
+        1,
+        0,
+        ahrs.get_error_rp(),
+        ahrs.get_error_yaw());
 }
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
 
@@ -275,7 +275,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
         chan,
         (float)g_gps->ground_speed / 100.0,
         (float)g_gps->ground_speed / 100.0,
-        (dcm.yaw_sensor / 100) % 360,
+        (ahrs.yaw_sensor / 100) % 360,
         g.rc_3.servo_out/10,
         current_loc.alt / 100.0,
         climb_rate);
@@ -471,16 +471,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_statustext(chan);
         break;
 
-    case MSG_DCM:
+    case MSG_AHRS:
 #if HIL_MODE != HIL_MODE_ATTITUDE
-        CHECK_PAYLOAD_SIZE(DCM);
-        send_dcm(chan);
+        CHECK_PAYLOAD_SIZE(AHRS);
+        send_ahrs(chan);
 #endif
         break;
 
     case MSG_SIMSTATE:
 #ifdef DESKTOP_BUILD
-        CHECK_PAYLOAD_SIZE(DCM);
+        CHECK_PAYLOAD_SIZE(SIMSTATE);
         send_simstate(chan);
 #endif
         break;
@@ -736,7 +736,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
 		}
 
 		if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
-			send_message(MSG_DCM);
+			send_message(MSG_AHRS);
 			send_message(MSG_HWSTATUS);
 		}
 	}
@@ -1559,8 +1559,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_attitude_t packet;
             mavlink_msg_attitude_decode(msg, &packet);
 
-            // set dcm hil sensor
-            dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+            // set AHRS hil sensor
+            ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
 
             // rad/sec

ArduCopter/Log.pde

@@ -617,11 +617,10 @@ static void Log_Write_Performance()
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
-	DataFlash.WriteByte(	dcm.gyro_sat_count);				//1
-	DataFlash.WriteByte(	imu.adc_constraints);				//2
-	DataFlash.WriteByte(	dcm.renorm_range_count);			//3
-	DataFlash.WriteByte(	dcm.renorm_blowup_count);			//4
-	DataFlash.WriteByte(	gps_fix_count);						//5
+	DataFlash.WriteByte(	imu.adc_constraints);				//1
+	DataFlash.WriteByte(	ahrs.renorm_range_count);			//2
+	DataFlash.WriteByte(	ahrs.renorm_blowup_count);			//3
+	DataFlash.WriteByte(	gps_fix_count);						//4
 	DataFlash.WriteByte(END_BYTE);
 }
 
@@ -632,15 +631,13 @@ static void Log_Read_Performance()
 	int8_t temp2 	= DataFlash.ReadByte();
 	int8_t temp3 	= DataFlash.ReadByte();
 	int8_t temp4 	= DataFlash.ReadByte();
-	int8_t temp5 	= DataFlash.ReadByte();
 
-							 //1   2   3   4   5
-	Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d\n"),
+							 //1   2   3   4
+	Serial.printf_P(PSTR("PM, %d, %d, %d, %d\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
-                    (int)temp4,
-                    (int)temp5);
+                    (int)temp4);
 }
 
 // Write a command processing packet.  Total length : 21 bytes
@@ -696,11 +693,11 @@ static void Log_Write_Attitude()
 	DataFlash.WriteByte(LOG_ATTITUDE_MSG);
 
 	DataFlash.WriteInt(g.rc_1.control_in);			// 1
-	DataFlash.WriteInt((int)dcm.roll_sensor);		// 2
+	DataFlash.WriteInt((int)ahrs.roll_sensor);		// 2
 	DataFlash.WriteInt(g.rc_2.control_in);			// 3
-	DataFlash.WriteInt((int)dcm.pitch_sensor);		// 4
+	DataFlash.WriteInt((int)ahrs.pitch_sensor);		// 4
 	DataFlash.WriteInt(g.rc_4.control_in);			// 5
-	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);	// 6
+	DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);	// 6
 	DataFlash.WriteInt((uint16_t)(wrap_360(ToDeg(compass.heading)*100)));	// 7
 
 	DataFlash.WriteByte(END_BYTE);

ArduCopter/defines.h

@@ -236,7 +236,7 @@ enum ap_message {
     MSG_NEXT_WAYPOINT,
     MSG_NEXT_PARAM,
     MSG_STATUSTEXT,
-    MSG_DCM,
+    MSG_AHRS,
     MSG_SIMSTATE,
     MSG_HWSTATUS,
     MSG_RETRY_DEFERRED // this must be last

ArduCopter/sensors.pde

@@ -93,7 +93,7 @@ static void init_compass()
         Serial.println_P(PSTR("COMPASS INIT ERROR"));
         return;
     }
-    dcm.set_compass(&compass);
+    ahrs.set_compass(&compass);
     compass.null_offsets_enable();
 }
 

ArduCopter/system.pde

@@ -565,7 +565,7 @@ static void set_failsafe(boolean mode)
 static void
 init_simple_bearing()
 {
-	initial_simple_bearing = dcm.yaw_sensor;
+	initial_simple_bearing = ahrs.yaw_sensor;
 }
 
 static void update_throttle_cruise()

ArduCopter/test.pde

@@ -935,7 +935,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 			while(1){
 				delay(100);
 				if (compass.read()) {
-					compass.calculate(dcm.get_dcm_matrix());
+					compass.calculate(ahrs.get_dcm_matrix());
 					Vector3f maggy = compass.get_offsets();
 					Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
 									(wrap_360(ToDeg(compass.heading) * 100)) /100,