Merge branch 'master' of https://code.google.com/p/ardupilot-mega

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_AHRS.h>
-#include <AP_DCM.h>         // ArduPilot Mega DCM Library
 #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
+ AP_AHRS_Quaternion ahrs(&imu, g_gps_null);
-  // decide to actually use something else, I don't want the patch
-  // size to be huge
- AP_Quaternion dcm(&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
@@ -2210,6 +2206,9 @@ static void update_nav_wp()
 		nav_lon	= g.pid_loiter_rate_lon.get_integrator();
 		nav_lat = g.pid_loiter_rate_lon.get_integrator();
 
+		nav_lon			= constrain(nav_lon, -2000, 2000); 			// 20°
+		nav_lat			= constrain(nav_lat, -2000, 2000); 			// 20°
+
 		// rotate pitch and roll to the copter frame of reference
 		calc_loiter_pitch_roll();
 	}

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
@@ -255,7 +255,6 @@ static void reset_I_all(void)
 {
 	reset_rate_I();
 	reset_stability_I();
-	reset_nav_I();
 	reset_wind_I();
 	reset_throttle_I();
 	reset_optflow_I();
@@ -282,13 +281,14 @@ static void reset_optflow_I(void)
 static void reset_wind_I(void)
 {
 	// Wind Compensation
+	// this i is not currently being used, but we reset it anyway
+	// because someone may modify it and not realize it, causing a bug
 	g.pi_loiter_lat.reset_I();
 	g.pi_loiter_lon.reset_I();
-}
 
-static void reset_nav_I(void)
+	g.pid_loiter_rate_lat.reset_I();
-{
+	g.pid_loiter_rate_lon.reset_I();
-	// Rate control for WP navigation
+
 	g.pid_nav_lat.reset_I();
 	g.pid_nav_lon.reset_I();
 }
@@ -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,
+        ahrs.roll,
-        dcm.pitch,
+        ahrs.pitch,
-        dcm.yaw,
+        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();
+    Vector3f omega_I = ahrs.get_gyro_drift();
-    mavlink_msg_dcm_send(
+    mavlink_msg_ahrs_send(
         chan,
         omega_I.x,
         omega_I.y,
         omega_I.z,
-        dcm.get_accel_weight(),
+        1,
-        dcm.get_renorm_val(),
+        0,
-        dcm.get_error_rp(),
+        ahrs.get_error_rp(),
-        dcm.get_error_yaw());
+        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);
+        CHECK_PAYLOAD_SIZE(AHRS);
-        send_dcm(chan);
+        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);
 		}
 	}
@@ -1470,19 +1470,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					Log_Write_Data(1, ((AP_Int32 *)vp)->get());
 #endif
 					if (packet.param_value < 0) rounding_addition = -rounding_addition;
-					((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
+                    float v = packet.param_value+rounding_addition;
+                    v = constrain(v, -2147483648, 2147483647);
+					((AP_Int32 *)vp)->set_and_save(v);
                 } else if (var_type == AP_PARAM_INT16) {
 #if LOGGING_ENABLED == ENABLED
 					Log_Write_Data(3, (int32_t)((AP_Int16 *)vp)->get());
 #endif
 					if (packet.param_value < 0) rounding_addition = -rounding_addition;
-					((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
+                    float v = packet.param_value+rounding_addition;
+                    v = constrain(v, -32768, 32767);
+					((AP_Int16 *)vp)->set_and_save(v);
                 } else if (var_type == AP_PARAM_INT8) {
 #if LOGGING_ENABLED == ENABLED
 					Log_Write_Data(4, (int32_t)((AP_Int8 *)vp)->get());
 #endif
 					if (packet.param_value < 0) rounding_addition = -rounding_addition;
-					((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
+                    float v = packet.param_value+rounding_addition;
+                    v = constrain(v, -128, 127);
+					((AP_Int8 *)vp)->set_and_save(v);
 				} else {
 					// we don't support mavlink set on this parameter
 					break;
@@ -1559,8 +1565,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_attitude_t packet;
             mavlink_msg_attitude_decode(msg, &packet);
 
-            // set dcm hil sensor
+            // set AHRS hil sensor
-            dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+            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);				//1
-	DataFlash.WriteByte(	imu.adc_constraints);				//2
+	DataFlash.WriteByte(	ahrs.renorm_range_count);			//2
-	DataFlash.WriteByte(	dcm.renorm_range_count);			//3
+	DataFlash.WriteByte(	ahrs.renorm_blowup_count);			//3
-	DataFlash.WriteByte(	dcm.renorm_blowup_count);			//4
+	DataFlash.WriteByte(	gps_fix_count);						//4
-	DataFlash.WriteByte(	gps_fix_count);						//5
 	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
+							 //1   2   3   4
-	Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d\n"),
+	Serial.printf_P(PSTR("PM, %d, %d, %d, %d\n"),
                     (int)temp1,
                     (int)temp2,
                     (int)temp3,
-                    (int)temp4,
+                    (int)temp4);
-                    (int)temp5);
 }
 
 // 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

@@ -532,8 +532,6 @@ static void set_mode(byte mode)
 		reset_nav_params();
 		// remove the wind compenstaion
 		reset_wind_I();
-		// Clears the WP navigation speed compensation
-		reset_nav_I();
 		// Clears the alt hold compensation
 		reset_throttle_I();
 	}
@@ -567,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,

ArduPlane/ArduPlane.pde

@@ -40,8 +40,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
 #include <AP_IMU.h>         // ArduPilot Mega IMU Library
-#include <AP_DCM.h>         // ArduPilot Mega DCM Library
+#include <AP_AHRS.h>         // ArduPilot Mega DCM Library
-#include <AP_Quaternion.h>  // Madgwick quaternion system
 #include <PID.h>            // PID library
 #include <RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder.h>	// Range finder library
@@ -194,12 +193,9 @@ AP_GPS_None     g_gps_driver(NULL);
 AP_IMU_INS imu( &ins );
 
 #if QUATERNION_ENABLE == ENABLED
-  // this shouldn't be called dcm of course, but until we
+  AP_AHRS_Quaternion ahrs(&imu, g_gps);
-  // decide to actually use something else, I don't want the patch
-  // size to be huge
-  AP_Quaternion dcm(&imu, g_gps);
 #else
- AP_DCM  dcm(&imu, g_gps);
+  AP_AHRS_DCM  ahrs(&imu, g_gps);
 #endif
 
 #elif HIL_MODE == HIL_MODE_SENSORS
@@ -210,14 +206,14 @@ AP_Compass_HIL          compass;
 AP_GPS_HIL              g_gps_driver(NULL);
 AP_InertialSensor_Oilpan ins( &adc );
 AP_IMU_Shim imu;
-AP_DCM  dcm(&imu, g_gps);
+AP_AHRS_DCM  ahrs(&imu, g_gps);
 
 #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_IMU_Shim             imu; // never used
 AP_Baro_BMP085_HIL      barometer;
 
 #else
@@ -261,7 +257,7 @@ AP_Relay relay;
 // Camera/Antenna mount tracking and stabilisation stuff
 // --------------------------------------
 #if MOUNT == ENABLED
-AP_Mount camera_mount(g_gps, &dcm);
+AP_Mount camera_mount(g_gps, &ahrs);
 #endif
 
 
@@ -719,18 +715,18 @@ static void fast_loop()
 	}
 
 	#if HIL_MODE == HIL_MODE_SENSORS
-		// update hil before dcm update
+		// update hil before AHRS update
 		gcs_update();
 	#endif
 
-    dcm.update_DCM();
+    ahrs.update();
 
 	// uses the yaw from the DCM to give more accurate turns
 	calc_bearing_error();
 
 	# if HIL_MODE == HIL_MODE_DISABLED
 		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
-			Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
+			Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
 
 		if (g.log_bitmask & MASK_LOG_RAW)
 			Log_Write_Raw();
@@ -784,19 +780,19 @@ static void medium_loop()
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
             if (g.compass_enabled && compass.read()) {
-                dcm.set_compass(&compass);
+                ahrs.set_compass(&compass);
                 // Calculate heading
-                Matrix3f m = dcm.get_dcm_matrix();
+                Matrix3f m = ahrs.get_dcm_matrix();
                 compass.calculate(m);
                 compass.null_offsets(m);
             } else {
-                dcm.set_compass(NULL);
+                ahrs.set_compass(NULL);
             }
 			#endif
 /*{
-Serial.print(dcm.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(dcm.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
-Serial.print(dcm.yaw_sensor, DEC);	Serial.printf_P(PSTR("\t"));
+Serial.print(ahrs.yaw_sensor, DEC);	Serial.printf_P(PSTR("\t"));
 Vector3f tempaccel = imu.get_accel();
 Serial.print(tempaccel.x, DEC);	Serial.printf_P(PSTR("\t"));
 Serial.print(tempaccel.y, DEC);	Serial.printf_P(PSTR("\t"));
@@ -850,7 +846,7 @@ Serial.println(tempaccel.z, DEC);
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
 				if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
-					Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
+					Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
 
 				if (g.log_bitmask & MASK_LOG_CTUN)
 					Log_Write_Control_Tuning();

ArduPlane/Attitude.pde

@@ -38,7 +38,7 @@ static void stabilize()
         // handle this is to ensure both go in the same direction from
         // zero
         nav_roll += 18000;
-        if (dcm.roll_sensor < 0) nav_roll -= 36000;
+        if (ahrs.roll_sensor < 0) nav_roll -= 36000;
     }
 
 	// For Testing Only
@@ -48,11 +48,11 @@ static void stabilize()
 
 	// Calculate dersired servo output for the roll
 	// ---------------------------------------------
-	g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);
+	g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), delta_ms_fast_loop, speed_scaler);
 	long tempcalc = nav_pitch +
-	        fabs(dcm.roll_sensor * g.kff_pitch_compensation) +
+	        fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
 	        (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
-	        (dcm.pitch_sensor - g.pitch_trim);
+	        (ahrs.pitch_sensor - g.pitch_trim);
     if (inverted_flight) {
         // when flying upside down the elevator control is inverted
         tempcalc = -tempcalc;
@@ -120,7 +120,7 @@ static void stabilize()
 
 static void crash_checker()
 {
-	if(dcm.pitch_sensor < -4500){
+	if(ahrs.pitch_sensor < -4500){
 		crash_timer = 255;
 	}
 	if(crash_timer > 0)
@@ -215,7 +215,7 @@ static void calc_nav_roll()
 	nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
 
 	Vector3f omega;
-	omega = dcm.get_gyro();
+	omega = ahrs.get_gyro();
 
 	// rate limiter
 	long rate		= degrees(omega.z) * 100; 										// 3rad = 17188 , 6rad = 34377

ArduPlane/GCS_Mavlink.pde

@@ -108,13 +108,13 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
 
 static NOINLINE void send_attitude(mavlink_channel_t chan)
 {
-    Vector3f omega = dcm.get_gyro();
+    Vector3f omega = ahrs.get_gyro();
     mavlink_msg_attitude_send(
         chan,
         micros(),
-        dcm.roll,
+        ahrs.roll,
-        dcm.pitch - radians(g.pitch_trim*0.01),
+        ahrs.pitch - radians(g.pitch_trim*0.01),
-        dcm.yaw,
+        ahrs.yaw,
         omega.x,
         omega.y,
         omega.z);
@@ -306,7 +306,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,
         millis(),
@@ -434,7 +434,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
         chan,
         (float)airspeed / 100.0,
         (float)g_gps->ground_speed / 100.0,
-        (dcm.yaw_sensor / 100) % 360,
+        (ahrs.yaw_sensor / 100) % 360,
         (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
         current_loc.alt / 100.0,
         0);
@@ -486,18 +486,18 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
                                     imu.ax(), imu.ay(), imu.az());
 }
 
-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();
+    Vector3f omega_I = ahrs.get_gyro_drift();
-    mavlink_msg_dcm_send(
+    mavlink_msg_ahrs_send(
         chan,
         omega_I.x,
         omega_I.y,
         omega_I.z,
-        dcm.get_accel_weight(),
+        0,
-        dcm.get_renorm_val(),
+        0,
-        dcm.get_error_rp(),
+        ahrs.get_error_rp(),
-        dcm.get_error_yaw());
+        ahrs.get_error_yaw());
 }
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE
@@ -679,16 +679,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         break;
 #endif
 
-    case MSG_DCM:
+    case MSG_AHRS:
 #if HIL_MODE != HIL_MODE_ATTITUDE
-        CHECK_PAYLOAD_SIZE(DCM);
+        CHECK_PAYLOAD_SIZE(AHRS);
-        send_dcm(chan);
+        send_ahrs(chan);
 #endif
         break;
 
     case MSG_SIMSTATE:
 #ifdef DESKTOP_BUILD
-        CHECK_PAYLOAD_SIZE(DCM);
+        CHECK_PAYLOAD_SIZE(SIMSTATE);
         send_simstate(chan);
 #endif
         break;
@@ -932,7 +932,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);
 		}
 	}
@@ -1791,13 +1791,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     ((AP_Float *)vp)->set_and_save(packet.param_value);
                 } else if (var_type == AP_PARAM_INT32) {
                     if (packet.param_value < 0) rounding_addition = -rounding_addition;
-                    ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition);
+                    float v = packet.param_value+rounding_addition;
+                    v = constrain(v, -2147483648, 2147483647);
+					((AP_Int32 *)vp)->set_and_save(v);
                 } else if (var_type == AP_PARAM_INT16) {
                     if (packet.param_value < 0) rounding_addition = -rounding_addition;
-                    ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition);
+                    float v = packet.param_value+rounding_addition;
+                    v = constrain(v, -32768, 32767);
+					((AP_Int16 *)vp)->set_and_save(v);
                 } else if (var_type == AP_PARAM_INT8) {
                     if (packet.param_value < 0) rounding_addition = -rounding_addition;
-                    ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition);
+                    float v = packet.param_value+rounding_addition;
+                    v = constrain(v, -128, 127);
+					((AP_Int8 *)vp)->set_and_save(v);
                 } else {
                     // we don't support mavlink set on this parameter
                     break;
@@ -1928,8 +1934,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			
 			#else
 
-			// set dcm hil sensor
+			// set AHRS hil sensor
-            dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+            ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
 
 			#endif
@@ -1945,8 +1951,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_attitude_t packet;
             mavlink_msg_attitude_decode(msg, &packet);
 
-            // set dcm hil sensor
+            // set AHRS hil sensor
-            dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+            ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
             break;
         }

ArduPlane/Log.pde

@@ -239,15 +239,15 @@ static void Log_Write_Performance()
 	DataFlash.WriteLong(millis()- perf_mon_timer);
 	DataFlash.WriteInt((int16_t)mainLoop_count);
 	DataFlash.WriteInt(G_Dt_max);
-	DataFlash.WriteByte(dcm.gyro_sat_count);
+	DataFlash.WriteByte(0);
 	DataFlash.WriteByte(imu.adc_constraints);
-	DataFlash.WriteByte(dcm.renorm_range_count);
+	DataFlash.WriteByte(ahrs.renorm_range_count);
-	DataFlash.WriteByte(dcm.renorm_blowup_count);
+	DataFlash.WriteByte(ahrs.renorm_blowup_count);
 	DataFlash.WriteByte(gps_fix_count);
-	DataFlash.WriteInt((int)(dcm.get_health() * 1000));
+	DataFlash.WriteInt(1); // AHRS health
-	DataFlash.WriteInt((int)(dcm.get_gyro_drift().x * 1000));
+	DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
-	DataFlash.WriteInt((int)(dcm.get_gyro_drift().y * 1000));
+	DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
-	DataFlash.WriteInt((int)(dcm.get_gyro_drift().z * 1000));
+	DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
 	DataFlash.WriteInt(pmTest1);
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -300,10 +300,10 @@ static void Log_Write_Control_Tuning()
 	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
 	DataFlash.WriteInt((int)(g.channel_roll.servo_out));
 	DataFlash.WriteInt((int)nav_roll);
-	DataFlash.WriteInt((int)dcm.roll_sensor);
+	DataFlash.WriteInt((int)ahrs.roll_sensor);
 	DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
 	DataFlash.WriteInt((int)nav_pitch);
-	DataFlash.WriteInt((int)dcm.pitch_sensor);
+	DataFlash.WriteInt((int)ahrs.pitch_sensor);
 	DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
 	DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
 	DataFlash.WriteInt((int)(accel.y * 10000));
@@ -317,7 +317,7 @@ static void Log_Write_Nav_Tuning()
 	DataFlash.WriteByte(HEAD_BYTE1);
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
-	DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
+	DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
 	DataFlash.WriteInt((int)wp_distance);
 	DataFlash.WriteInt((uint16_t)target_bearing);
 	DataFlash.WriteInt((uint16_t)nav_bearing);

ArduPlane/commands_logic.pde

@@ -279,7 +279,7 @@ static bool verify_takeoff()
 		if (hold_course == -1) {
 			// save our current course to take off
 			if(g.compass_enabled) {
-				hold_course = dcm.yaw_sensor;
+				hold_course = ahrs.yaw_sensor;
 			} else {
 				hold_course = g_gps->ground_course;
 			}
@@ -326,7 +326,7 @@ static bool verify_land()
 			// be quite large at this point, and that could induce a
 			// sudden large roll correction which is very nasty at
 			// this point in the landing.
-			hold_course = dcm.yaw_sensor;
+			hold_course = ahrs.yaw_sensor;
 		}
 	}
 

ArduPlane/defines.h

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

ArduPlane/navigation.pde

@@ -115,7 +115,7 @@ static void calc_bearing_error()
           correction using the GPS typically takes 10 seconds or so
           for a 180 degree correction.
          */
-		bearing_error = nav_bearing - dcm.yaw_sensor;
+		bearing_error = nav_bearing - ahrs.yaw_sensor;
 	} else {
 
 		// TODO: we need to use the Yaw gyro for in between GPS reads,

ArduPlane/system.pde

@@ -183,7 +183,7 @@ static void init_ardupilot()
             Serial.println_P(PSTR("Compass initialisation failed!"));
             g.compass_enabled = false;
         } else {
-            dcm.set_compass(&compass);
+            ahrs.set_compass(&compass);
             compass.null_offsets_enable();
         }
 	}
@@ -263,7 +263,7 @@ static void init_ardupilot()
 		//read_EEPROM_airstart_critical();
 #if HIL_MODE != HIL_MODE_ATTITUDE
 		imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
-		dcm.set_centripetal(1);
+		ahrs.set_centripetal(1);
 #endif
 
 		// This delay is important for the APM_RC library to work.
@@ -455,8 +455,8 @@ static void startup_IMU_ground(void)
 
 	imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
 	imu.init_accel(mavlink_delay, flash_leds);
-	dcm.set_centripetal(1);
+	ahrs.set_centripetal(1);
-    dcm.matrix_reset();
+    ahrs.reset();
 
 	// read Baro pressure at ground
 	//-----------------------------
@@ -510,10 +510,9 @@ static void update_GPS_light(void)
 static void resetPerfData(void) {
 	mainLoop_count 			= 0;
 	G_Dt_max 				= 0;
-	dcm.gyro_sat_count 		= 0;
 	imu.adc_constraints 	= 0;
-	dcm.renorm_range_count 	= 0;
+	ahrs.renorm_range_count 	= 0;
-	dcm.renorm_blowup_count = 0;
+	ahrs.renorm_blowup_count = 0;
 	gps_fix_count 			= 0;
 	pmTest1					= 0;
 	perf_mon_timer 			= millis();

ArduPlane/test.pde

@@ -514,7 +514,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 {
 	//Serial.printf_P(PSTR("Calibrating."));
 	imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
-    dcm.matrix_reset();
+    ahrs.reset();
 
 	print_hit_enter();
 	delay(1000);
@@ -528,14 +528,14 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 
 			// IMU
 			// ---
-			dcm.update_DCM();
+			ahrs.update();
 
 			if(g.compass_enabled) {
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
 					if (compass.read()) {
                         // Calculate heading
-                        compass.calculate(dcm.get_dcm_matrix());
+                        compass.calculate(ahrs.get_dcm_matrix());
                     }
                     medium_loopCounter = 0;
 				}
@@ -546,9 +546,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
             Vector3f gyros 	= imu.get_gyro();
             Vector3f accels = imu.get_accel();
 			Serial.printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
-                            (int)dcm.roll_sensor / 100,
+                            (int)ahrs.roll_sensor / 100,
-                            (int)dcm.pitch_sensor / 100,
+                            (int)ahrs.pitch_sensor / 100,
-                            (uint16_t)dcm.yaw_sensor / 100,
+                            (uint16_t)ahrs.yaw_sensor / 100,
                             gyros.x, gyros.y, gyros.z,
                             accels.x, accels.y, accels.z);
 		}
@@ -574,12 +574,12 @@ test_mag(uint8_t argc, const Menu::arg *argv)
         return 0;
     }
     compass.null_offsets_enable();
-    dcm.set_compass(&compass);
+    ahrs.set_compass(&compass);
     report_compass();
 
-    // we need the DCM initialised for this test
+    // we need the AHRS initialised for this test
 	imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
-    dcm.matrix_reset();
+    ahrs.reset();
 
 	int counter = 0;
 		//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
@@ -595,13 +595,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
 			// IMU
 			// ---
-			dcm.update_DCM();
+			ahrs.update();
 
             medium_loopCounter++;
             if(medium_loopCounter == 5){
                 if (compass.read()) {
                     // Calculate heading
-                    Matrix3f m = dcm.get_dcm_matrix();
+                    Matrix3f m = ahrs.get_dcm_matrix();
                     compass.calculate(m);
                     compass.null_offsets(m);
                 }

Tools/VARTest/VARTest.pde

@@ -15,7 +15,7 @@
 #include <SPI.h>			// Arduino SPI lib
 #include <I2C.h>
 #include <DataFlash.h>
-#include <AP_DCM.h>
+#include <AP_AHRS.h>
 #include <AP_ADC.h>
 #include <AP_Baro.h>
 #include <Filter.h>
@@ -25,6 +25,7 @@
 #include <AP_IMU.h>         // ArduPilot Mega IMU Library
 #include <AP_GPS.h>
 #include <AP_Math.h>
+#include <GCS_MAVLink.h>
 #include <config.h>
 #include <Parameters.h>
 
@@ -43,7 +44,7 @@ AP_GPS_Auto     g_gps_driver(&Serial1, &g_gps);
   AP_InertialSensor_Oilpan ins( &adc );
 #endif // CONFIG_IMU_TYPE
 AP_IMU_INS imu( &ins );
-AP_DCM  dcm(&imu, g_gps);
+AP_AHRS_DCM  ahrs(&imu, g_gps);
 
 Arduino_Mega_ISR_Registry isr_registry;
 #ifdef DESKTOP_BUILD

libraries/AP_AHRS/AP_AHRS.h

@@ -0,0 +1,109 @@
+#ifndef AP_AHRS_H
+#define AP_AHRS_H
+/*
+  AHRS (Attitude Heading Reference System) interface for ArduPilot
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+*/
+
+#include <AP_Math.h>
+#include <inttypes.h>
+#include <AP_Compass.h>
+#include <AP_GPS.h>
+#include <AP_IMU.h>
+
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "WProgram.h"
+#endif
+
+class AP_AHRS
+{
+public:
+	// Constructor
+	AP_AHRS(IMU *imu, GPS *&gps):
+		_imu(imu),
+		_gps(gps)
+	{
+		// base the ki values by the sensors maximum drift
+		// rate. The APM2 has gyros which are much less drift
+		// prone than the APM1, so we should have a lower ki,
+		// which will make us less prone to increasing omegaI
+		// incorrectly due to sensor noise
+		_gyro_drift_limit = imu->get_gyro_drift_rate();
+	}
+
+	// Accessors
+	void		set_centripetal(bool b) { _centripetal = b; }
+	bool		get_centripetal(void) { return _centripetal; }
+	void		set_compass(Compass *compass) { _compass = compass; }
+
+	// Methods
+	virtual void update(void) = 0;
+
+	// Euler angles (radians)
+	float		roll;
+	float		pitch;
+	float		yaw;
+
+	// integer Euler angles (Degrees * 100)
+	int32_t		roll_sensor;
+	int32_t		pitch_sensor;
+	int32_t		yaw_sensor;
+
+	// return a smoothed and corrected gyro vector
+	virtual Vector3f get_gyro(void) = 0;
+
+	// return the current estimate of the gyro drift
+	virtual Vector3f get_gyro_drift(void) = 0;
+
+	// reset the current attitude, used on new IMU calibration
+	virtual void reset(bool recover_eulers=false) = 0;
+
+	// how often our attitude representation has gone out of range
+	uint8_t renorm_range_count;
+
+	// how often our attitude representation has blown up completely
+	uint8_t renorm_blowup_count;
+
+	// return the average size of the roll/pitch error estimate
+	// since last call
+	virtual float get_error_rp(void) = 0;
+
+	// return the average size of the yaw error estimate
+	// since last call
+	virtual float get_error_yaw(void) = 0;
+
+	// return a DCM rotation matrix representing our current
+	// attitude
+	virtual Matrix3f get_dcm_matrix(void) = 0;
+
+protected:
+	// pointer to compass object, if enabled
+	Compass 	* _compass;
+
+	// time in microseconds of last compass update
+	uint32_t        _compass_last_update;
+
+	// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
+	//       IMU under us without our noticing.
+	GPS 		*&_gps;
+	IMU 		*_imu;
+
+	// true if we are doing centripetal acceleration correction
+	bool		_centripetal;
+
+	// the limit of the gyro drift claimed by the sensors, in
+	// radians/s/s
+	float           _gyro_drift_limit;
+};
+
+#include <AP_AHRS_DCM.h>
+#include <AP_AHRS_Quaternion.h>
+#include <AP_AHRS_HIL.h>
+
+#endif // AP_AHRS_H

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -1,22 +1,19 @@
 /*
-	APM_DCM.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
+	APM_AHRS_DCM.cpp
-		Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
 
-	This library works with the ArduPilot Mega and "Oilpan"
+	AHRS system using DCM matrices
+
+	Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
+
+	Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
 
 	This library is free software; you can redistribute it and/or
-    modify it under the terms of the GNU Lesser General Public
+	modify it under the terms of the GNU Lesser General Public License
-    License as published by the Free Software Foundation; either
+	as published by the Free Software Foundation; either version 2.1
-    version 2.1 of the License, or (at your option) any later version.
+	of the License, or (at your option) any later version.
-
-        Methods:
-				update_DCM()	: Updates the AHRS by integrating the rotation matrix over time using the IMU object data
-				get_gyro()			: Returns gyro vector corrected for bias
-				get_accel()		: Returns accelerometer vector
-				get_dcm_matrix()	: Returns dcm matrix
-
 */
-#include <AP_DCM.h>
+#include <FastSerial.h>
+#include <AP_AHRS.h>
 
 // this is the speed in cm/s above which we first get a yaw lock with
 // the GPS
@@ -26,21 +23,11 @@
 // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
 #define GPS_SPEED_RESET 100
 
-void
-AP_DCM::set_compass(Compass *compass)
-{
-	_compass = compass;
-}
-
-
 // run a full DCM update round
-// the drift_correction_frequency is how many steps of the algorithm
-// to run before doing an accelerometer and yaw drift correction step
 void
-AP_DCM::update_DCM(uint8_t drift_correction_frequency)
+AP_AHRS_DCM::update(void)
 {
 	float delta_t;
-	Vector3f accel;
 
 	// tell the IMU to grab some data
 	_imu->update();
@@ -50,43 +37,16 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
 
 	// Get current values for gyros
 	_gyro_vector  = _imu->get_gyro();
-
+	_accel_vector = _imu->get_accel();
-	// accumulate some more accelerometer data
-	accel = _imu->get_accel();
-	_accel_sum += accel;
-	_drift_correction_time += delta_t;
 
 	// Integrate the DCM matrix using gyro inputs
 	matrix_update(delta_t);
 
-	// add up the omega vector so we pass a value to the drift
-	// correction averaged over the same time period as the
-	// accelerometers
-	_omega_sum += _omega_integ_corr;
-
 	// Normalize the DCM matrix
 	normalize();
 
-	// see if we will perform drift correction on this call
+	// Perform drift correction
-	_drift_correction_count++;
+	drift_correction(delta_t);
-
-	if (_drift_correction_count >= drift_correction_frequency) {
-		// calculate the average accelerometer vector over
-		// since the last drift correction call
-		float scale = 1.0 / _drift_correction_count;
-		_accel_vector = _accel_sum * scale;
-		_accel_sum.zero();
-
-		// calculate the average omega value over this time
-		_omega_smoothed = _omega_sum * scale;
-		_omega_sum.zero();
-
-		// Perform drift correction
-		drift_correction(_drift_correction_time);
-
-		_drift_correction_time = 0;
-		_drift_correction_count = 0;
-	}
 
 	// paranoid check for bad values in the DCM matrix
 	check_matrix();
@@ -97,7 +57,7 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
 
 // update the DCM matrix using only the gyros
 void
-AP_DCM::matrix_update(float _G_Dt)
+AP_AHRS_DCM::matrix_update(float _G_Dt)
 {
 	// _omega_integ_corr is used for _centripetal correction
 	// (theoretically better than _omega)
@@ -106,33 +66,22 @@ AP_DCM::matrix_update(float _G_Dt)
 	// Equation 16, adding proportional and integral correction terms
 	_omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
 
-	// this is an expansion of the DCM matrix multiply (equation
+	// this is a replacement of the DCM matrix multiply (equation
 	// 17), with known zero elements removed and the matrix
 	// operations inlined. This runs much faster than the original
 	// version of this code, as the compiler was doing a terrible
 	// job of realising that so many of the factors were in common
 	// or zero. It also uses much less stack, as we no longer need
-	// additional local matrices
+	// two additional local matrices
 
-	float tmpx = _G_Dt * _omega.x;
+	Vector3f r = _omega * _G_Dt;
-	float tmpy = _G_Dt * _omega.y;
+	_dcm_matrix.rotate(r);
-	float tmpz = _G_Dt * _omega.z;
-
-	_dcm_matrix.a.x += _dcm_matrix.a.y * tmpz  - _dcm_matrix.a.z * tmpy;
-	_dcm_matrix.a.y += _dcm_matrix.a.z * tmpx  - _dcm_matrix.a.x * tmpz;
-	_dcm_matrix.a.z += _dcm_matrix.a.x * tmpy  - _dcm_matrix.a.y * tmpx;
-	_dcm_matrix.b.x += _dcm_matrix.b.y * tmpz  - _dcm_matrix.b.z * tmpy;
-	_dcm_matrix.b.y += _dcm_matrix.b.z * tmpx  - _dcm_matrix.b.x * tmpz;
-	_dcm_matrix.b.z += _dcm_matrix.b.x * tmpy  - _dcm_matrix.b.y * tmpx;
-	_dcm_matrix.c.x += _dcm_matrix.c.y * tmpz  - _dcm_matrix.c.z * tmpy;
-	_dcm_matrix.c.y += _dcm_matrix.c.z * tmpx  - _dcm_matrix.c.x * tmpz;
-	_dcm_matrix.c.z += _dcm_matrix.c.x * tmpy  - _dcm_matrix.c.y * tmpx;
 }
 
 
 // adjust an accelerometer vector for known acceleration forces
 void
-AP_DCM::accel_adjust(Vector3f &accel)
+AP_AHRS_DCM::accel_adjust(Vector3f &accel)
 {
 	float veloc;
 	// compensate for linear acceleration. This makes a
@@ -149,8 +98,8 @@ AP_DCM::accel_adjust(Vector3f &accel)
 	// direction as positive
 
 	// Equation 26 broken up into separate pieces
-	accel.y -= _omega_smoothed.z * veloc;
+	accel.y -= _omega_integ_corr.z * veloc;
-	accel.z += _omega_smoothed.y * veloc;
+	accel.z += _omega_integ_corr.y * veloc;
 }
 
 /*
@@ -158,10 +107,10 @@ AP_DCM::accel_adjust(Vector3f &accel)
   extreme errors in the matrix
  */
 void
-AP_DCM::matrix_reset(bool recover_eulers)
+AP_AHRS_DCM::reset(bool recover_eulers)
 {
 	if (_compass != NULL) {
-		_compass->null_offsets_disable();		
+		_compass->null_offsets_disable();
 	}
 
 	// reset the integration terms
@@ -169,7 +118,6 @@ AP_DCM::matrix_reset(bool recover_eulers)
 	_omega_P.zero();
 	_omega_yaw_P.zero();
 	_omega_integ_corr.zero();
-	_omega_smoothed.zero();
 	_omega.zero();
 
 	// if the caller wants us to try to recover to the current
@@ -193,13 +141,13 @@ AP_DCM::matrix_reset(bool recover_eulers)
   check the DCM matrix for pathological values
  */
 void
-AP_DCM::check_matrix(void)
+AP_AHRS_DCM::check_matrix(void)
 {
 	if (_dcm_matrix.is_nan()) {
 		//Serial.printf("ERROR: DCM matrix NAN\n");
 		SITL_debug("ERROR: DCM matrix NAN\n");
 		renorm_blowup_count++;
-		matrix_reset(true);
+		reset(true);
 		return;
 	}
 	// some DCM matrix values can lead to an out of range error in
@@ -221,7 +169,7 @@ AP_DCM::check_matrix(void)
 			SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
 				   _dcm_matrix.c.x);
 			renorm_blowup_count++;
-			matrix_reset(true);
+			reset(true);
 		}
 	}
 }
@@ -229,7 +177,7 @@ AP_DCM::check_matrix(void)
 // renormalise one vector component of the DCM matrix
 // this will return false if renormalization fails
 bool
-AP_DCM::renorm(Vector3f const &a, Vector3f &result)
+AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
 {
 	float	renorm_val;
 
@@ -289,7 +237,7 @@ simple matter to stay ahead of it.
 We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
 */
 void
-AP_DCM::normalize(void)
+AP_AHRS_DCM::normalize(void)
 {
 	float error;
 	Vector3f t0, t1, t2;
@@ -305,7 +253,7 @@ AP_DCM::normalize(void)
 	    !renorm(t2, _dcm_matrix.c)) {
 		// Our solution is blowing up and we will force back
 		// to last euler angles
-		matrix_reset(true);
+		reset(true);
 	}
 }
 
@@ -319,7 +267,7 @@ AP_DCM::normalize(void)
 // This function also updates _omega_yaw_P with a yaw correction term
 // from our yaw reference vector
 void
-AP_DCM::drift_correction(float deltat)
+AP_AHRS_DCM::drift_correction(float deltat)
 {
 	float error_course = 0;
 	Vector3f accel;
@@ -530,7 +478,7 @@ AP_DCM::drift_correction(float deltat)
 // calculate the euler angles which will be used for high level
 // navigation control
 void
-AP_DCM::euler_angles(void)
+AP_AHRS_DCM::euler_angles(void)
 {
 	_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
@@ -544,27 +492,8 @@ AP_DCM::euler_angles(void)
 
 /* reporting of DCM state for MAVLink */
 
-// average accel_weight since last call
-float AP_DCM::get_accel_weight(void)
-{
-	return 1.0;
-}
-
-// average renorm_val since last call
-float AP_DCM::get_renorm_val(void)
-{
-	float ret;
-	if (_renorm_val_count == 0) {
-		return 0;
-	}
-	ret = _renorm_val_sum / _renorm_val_count;
-	_renorm_val_sum = 0;
-	_renorm_val_count = 0;
-	return ret;
-}
-
 // average error_roll_pitch since last call
-float AP_DCM::get_error_rp(void)
+float AP_AHRS_DCM::get_error_rp(void)
 {
 	float ret;
 	if (_error_rp_count == 0) {
@@ -577,7 +506,7 @@ float AP_DCM::get_error_rp(void)
 }
 
 // average error_yaw since last call
-float AP_DCM::get_error_yaw(void)
+float AP_AHRS_DCM::get_error_yaw(void)
 {
 	float ret;
 	if (_error_yaw_count == 0) {

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -0,0 +1,85 @@
+#ifndef AP_AHRS_DCM_H
+#define AP_AHRS_DCM_H
+/*
+  DCM based AHRS (Attitude Heading Reference System) interface for
+  ArduPilot
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+*/
+
+class AP_AHRS_DCM : public AP_AHRS
+{
+public:
+	// Constructors
+	AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
+	{
+		_kp_roll_pitch = 0.13;
+		_kp_yaw        = 0.4;
+		_dcm_matrix(Vector3f(1, 0, 0),
+			    Vector3f(0, 1, 0),
+			    Vector3f(0, 0, 1));
+
+		// base the ki values on the sensors drift rate
+		_ki_roll_pitch = _gyro_drift_limit * 5;
+		_ki_yaw        = _gyro_drift_limit * 8;
+	}
+
+	// return the smoothed gyro vector corrected for drift
+	Vector3f	get_gyro(void) {return _omega; }
+	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
+
+	// return the current drift correction integrator value
+	Vector3f	get_gyro_drift(void) {return _omega_I; }
+
+	// Methods
+	void 		update(void);
+	void 		reset(bool recover_eulers = false);
+
+	// status reporting
+	float		get_error_rp(void);
+	float		get_error_yaw(void);
+
+private:
+	float		_kp_roll_pitch;
+	float		_ki_roll_pitch;
+	float		_kp_yaw;
+	float		_ki_yaw;
+	bool		_have_initial_yaw;
+
+	// Methods
+	void 		accel_adjust(Vector3f &accel);
+	void 		matrix_update(float _G_Dt);
+	void 		normalize(void);
+	void		check_matrix(void);
+	bool	 	renorm(Vector3f const &a, Vector3f &result);
+	void 		drift_correction(float deltat);
+	void 		euler_angles(void);
+
+	// primary representation of attitude
+	Matrix3f	_dcm_matrix;
+
+	Vector3f 	_gyro_vector;		// Store the gyros turn rate in a vector
+	Vector3f 	_accel_vector;		// current accel vector
+
+	Vector3f	_omega_P;		// accel Omega Proportional correction
+	Vector3f	_omega_yaw_P;		// yaw Omega Proportional correction
+	Vector3f 	_omega_I;		// Omega Integrator correction
+	Vector3f 	_omega_integ_corr;	// Partially corrected Gyro_Vector data - used for centrepetal correction
+	Vector3f 	_omega;			// Corrected Gyro_Vector data
+
+	// state to support status reporting
+	float		_renorm_val_sum;
+	uint16_t	_renorm_val_count;
+	float		_error_rp_sum;
+	uint16_t	_error_rp_count;
+	float		_error_yaw_sum;
+	uint16_t	_error_yaw_count;
+
+	// time in millis when we last got a GPS heading
+	uint32_t	_gps_last_update;
+};
+
+#endif // AP_AHRS_DCM_H

libraries/AP_AHRS/AP_AHRS_HIL.cpp

@@ -0,0 +1,30 @@
+/*
+	APM_AHRS_HIL.cpp
+
+	Hardware in the loop AHRS object
+
+	This library is free software; you can redistribute it and/or
+	modify it under the terms of the GNU Lesser General Public
+	License as published by the Free Software Foundation; either
+	version 2.1 of the License, or (at your option) any later
+	version.
+*/
+
+#include <FastSerial.h>
+#include <AP_AHRS.h>
+
+/**************************************************/
+void
+AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw,
+		    float _rollRate, float _pitchRate, float _yawRate)
+{
+	roll = _roll;
+	pitch = _pitch;
+	yaw = _yaw;
+
+	_omega(_rollRate, _pitchRate, _yawRate);
+
+	roll_sensor  = ToDeg(roll)*100;
+	pitch_sensor = ToDeg(pitch)*100;
+	yaw_sensor   = ToDeg(yaw)*100;
+}

libraries/AP_AHRS/AP_AHRS_HIL.h

@@ -0,0 +1,39 @@
+#ifndef AP_AHRS_HIL_H
+#define AP_AHRS_HIL_H
+
+class AP_AHRS_HIL : public AP_AHRS
+{
+public:
+	// Constructors
+	AP_AHRS_HIL(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
+	{
+	}
+
+	// Accessors
+	Vector3f get_gyro(void) {return _omega; }
+
+	Matrix3f get_dcm_matrix(void) {
+		Matrix3f m;
+		m.from_euler(roll, pitch, yaw);
+		return m;
+	}
+
+	// Methods
+	void 		update(void) {}
+	void 		setHil(float roll, float pitch, float yaw,
+			       float rollRate, float pitchRate, float yawRate);
+
+	// return the current estimate of the gyro drift
+	Vector3f get_gyro_drift(void) { return Vector3f(0,0,0); }
+
+	// reset the current attitude, used on new IMU calibration
+	void reset(bool recover_eulers=false) {}
+
+	float get_error_rp(void) { return 0; }
+	float get_error_yaw(void) { return 0; }
+
+private:
+	Vector3f 	_omega;
+};
+
+#endif

libraries/AP_AHRS/AP_AHRS_Quaternion.cpp

@@ -1,5 +1,5 @@
 /*
-	AP_Quaternion code, based on quaternion code from Jeb Madgwick
+	AP_AHRS_Quaternion code, based on quaternion code from Jeb Madgwick
 
 	See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
 
@@ -13,24 +13,7 @@
 	version.
 */
 #include <FastSerial.h>
-#include <AP_Quaternion.h>
+#include <AP_AHRS.h>
-
-#define ToRad(x) (x*0.01745329252)	// *pi/180
-#define ToDeg(x) (x*57.2957795131)	// *180/pi
-
-// this is the speed in cm/s above which we first get a yaw lock with
-// the GPS
-#define GPS_SPEED_MIN 300
-
-// this is the speed in cm/s at which we stop using drift correction
-// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
-#define GPS_SPEED_RESET 100
-
-void
-AP_Quaternion::set_compass(Compass *compass)
-{
-	_compass = compass;
-}
 
 // to keep the code as close to the original as possible, we use these
 // macros for quaternion access
@@ -40,7 +23,7 @@ AP_Quaternion::set_compass(Compass *compass)
 #define SEq_4 q.q4
 
 // Function to compute one quaternion iteration without magnetometer
-void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
+void AP_AHRS_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
 {
     // Local system variables
     float norm;                                                            // vector norm
@@ -134,7 +117,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
 
 
 // Function to compute one quaternion iteration including magnetometer
-void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
+void AP_AHRS_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
 {
     // local system variables
     float norm;                                                             //                vector norm
@@ -253,7 +236,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
 
     // don't allow the drift rate to be exceeded. This prevents a
     // sudden drift change coming from a outage in the compass
-    float max_change = gyroMeasDrift * deltat;
+    float max_change = _gyro_drift_limit * deltat;
     drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
     drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
     drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
@@ -308,161 +291,8 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
 }
 
 
-// Function to update our gyro_bias drift estimate using the accelerometer
-// and magnetometer. This is a cut-down version of update_MARG(), but
-// without the quaternion update.
-void AP_Quaternion::update_drift(float deltat, Vector3f &mag)
-{
-    // local system variables
-    float norm;                                                             //                vector norm
-    float f_4, f_5, f_6;                                     //                objective function elements
-    float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33,               //                objective function Jacobian elements
-    J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
-    float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;               //                estimated direction of the gyroscope error
-
-    // computed flux in the earth frame
-    Vector3f flux;
-
-    // estimated direction of the gyroscope error (radians)
-    Vector3f w_err;
-
-    // normalise the magnetometer measurement
-    mag.normalize();
-    if (mag.is_inf()) {
-	    // discard this data point
-	    renorm_range_count++;
-	    return;
-    }
-
-    // auxiliary variables to avoid repeated calculations
-    float twoSEq_1 = 2.0f * SEq_1;
-    float twoSEq_2 = 2.0f * SEq_2;
-    float twoSEq_3 = 2.0f * SEq_3;
-    float twoSEq_4 = 2.0f * SEq_4;
-    float twob_x = 2.0f * b_x;
-    float twob_z = 2.0f * b_z;
-    float twob_xSEq_1 = 2.0f * b_x * SEq_1;
-    float twob_xSEq_2 = 2.0f * b_x * SEq_2;
-    float twob_xSEq_3 = 2.0f * b_x * SEq_3;
-    float twob_xSEq_4 = 2.0f * b_x * SEq_4;
-    float twob_zSEq_1 = 2.0f * b_z * SEq_1;
-    float twob_zSEq_2 = 2.0f * b_z * SEq_2;
-    float twob_zSEq_3 = 2.0f * b_z * SEq_3;
-    float twob_zSEq_4 = 2.0f * b_z * SEq_4;
-    float SEq_1SEq_2;
-    float SEq_1SEq_3 = SEq_1 * SEq_3;
-    float SEq_1SEq_4;
-    float SEq_2SEq_3;
-    float SEq_2SEq_4 = SEq_2 * SEq_4;
-    float SEq_3SEq_4;
-    Vector3f twom = mag * 2.0;
-
-    // compute the objective function and Jacobian
-    f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x;
-    f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y;
-    f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z;
-    J_11or24 = twoSEq_3;                                  // J_11 negated in matrix multiplication
-    J_12or23 = 2.0f * SEq_4;
-    J_13or22 = twoSEq_1;                                  // J_12 negated in matrix multiplication
-    J_14or21 = twoSEq_2;
-    J_32 = 2.0f * J_14or21;                               // negated in matrix multiplication
-    J_33 = 2.0f * J_11or24;                               // negated in matrix multiplication
-    J_41 = twob_zSEq_3;                                   // negated in matrix multiplication
-    J_42 = twob_zSEq_4;
-    J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1;              // negated in matrix multiplication
-    J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2;              // negated in matrix multiplication
-    J_51 = twob_xSEq_4 - twob_zSEq_2;                     // negated in matrix multiplication
-    J_52 = twob_xSEq_3 + twob_zSEq_1;
-    J_53 = twob_xSEq_2 + twob_zSEq_4;
-    J_54 = twob_xSEq_1 - twob_zSEq_3;                     // negated in matrix multiplication
-    J_61 = twob_xSEq_3;
-    J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;
-    J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;
-    J_64 = twob_xSEq_2;
-
-    // compute the gradient (matrix multiplication)
-    SEqHatDot_1 = - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
-    SEqHatDot_2 = + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
-    SEqHatDot_3 = - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
-    SEqHatDot_4 = - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
-
-    // normalise the gradient to estimate direction of the gyroscope error
-    norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
-    if (isinf(norm)) {
-	    // discard this data point
-	    renorm_range_count++;
-	    return;
-    }
-    SEqHatDot_1 *= norm;
-    SEqHatDot_2 *= norm;
-    SEqHatDot_3 *= norm;
-    SEqHatDot_4 *= norm;
-
-    // compute angular estimated direction of the gyroscope error
-    w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
-    w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
-    w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
-
-    // keep track of the error rates
-    _error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y));
-    _error_yaw_sum += fabs(w_err.z);
-    _error_rp_count++;
-    _error_yaw_count++;
-
-    // compute the gyroscope bias delta
-    Vector3f drift_delta = w_err * (deltat * zeta);
-
-    // don't allow the drift rate to be exceeded. This prevents a
-    // sudden drift change coming from a outage in the compass
-    float max_change = gyroMeasDrift * deltat;
-    drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
-    drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
-    drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
-    gyro_bias += drift_delta;
-
-    // compute then integrate the estimated quaternion rate
-    SEq_1 -= (beta * SEqHatDot_1) * deltat;
-    SEq_2 -= (beta * SEqHatDot_2) * deltat;
-    SEq_3 -= (beta * SEqHatDot_3) * deltat;
-    SEq_4 -= (beta * SEqHatDot_4) * deltat;
-
-    // normalise quaternion
-    norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
-    if (isinf(norm)) {
-	    // our quaternion is bad! Reset based on roll/pitch/yaw
-	    // and hope for the best ...
-	    renorm_blowup_count++;
-	    _compass->null_offsets_disable();
-	    q.from_euler(roll, pitch, yaw);
-	    _compass->null_offsets_disable();
-	    return;
-    }
-    SEq_1 *= norm;
-    SEq_2 *= norm;
-    SEq_3 *= norm;
-    SEq_4 *= norm;
-
-    // compute flux in the earth frame
-    // recompute axulirary variables
-    SEq_1SEq_2 = SEq_1 * SEq_2;
-    SEq_1SEq_3 = SEq_1 * SEq_3;
-    SEq_1SEq_4 = SEq_1 * SEq_4;
-    SEq_3SEq_4 = SEq_3 * SEq_4;
-    SEq_2SEq_3 = SEq_2 * SEq_3;
-    SEq_2SEq_4 = SEq_2 * SEq_4;
-    flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3);
-    flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2);
-    flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);
-
-    // normalise the flux vector to have only components in the x and z
-    b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y));
-    b_z = flux.z;
-}
-
-
-
 // Function to compute one quaternion iteration
-void AP_Quaternion::update(void)
+void AP_AHRS_Quaternion::update(void)
 {
 	Vector3f gyro, accel;
 	float deltat;
@@ -491,22 +321,16 @@ void AP_Quaternion::update(void)
 
 	// get current IMU state
 	gyro = _imu->get_gyro();
-	accel = _imu->get_accel();
 
-	// Quaternion code uses opposite x and y gyro sense from the
+	// the quaternion system uses opposite sign for accel
-	// rest of APM
+	accel = - _imu->get_accel();
-	gyro.x = -gyro.x;
-	gyro.y = -gyro.y;
-
-	// Quaternion code uses opposite z accel as well
-	accel.z = -accel.z;
 
 	if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
 		// compensate for linear acceleration. This makes a
 		// surprisingly large difference in the pitch estimate when
 		// turning, plus on takeoff and landing
 		float acceleration = _gps->acceleration();
-		accel.x -= acceleration;
+		accel.x += acceleration;
 
 		// compensate for centripetal acceleration
 		float veloc;
@@ -514,46 +338,19 @@ void AP_Quaternion::update(void)
 		// be careful of the signs in this calculation. the
 		// quaternion system uses different signs than the
 		// rest of APM
-		accel.y -= (gyro.z - gyro_bias.z) * veloc;
+		accel.y += (gyro.z - gyro_bias.z) * veloc;
-		accel.z += (gyro.y - gyro_bias.y) * veloc;
+		accel.z -= (gyro.y - gyro_bias.y) * veloc;
 	}
 
-#define SEPARATE_DRIFT 0
+	if (_compass != NULL && _compass->use_for_yaw()) {
-#if SEPARATE_DRIFT
+		Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
-	/*
+		update_MARG(deltat, gyro, accel, mag);
-	  The full Madgwick quaternion 'MARG' system assumes you get
+	} else {
-	  gyro, accel and magnetometer updates at the same rate. On
+		// step the quaternion solution using just gyros and accels
-	  APM we get them at very different rates, and re-calculating
+		gyro -= gyro_bias;
-	  our drift due to the magnetometer in the fast loop is very
+		update_IMU(deltat, gyro, accel);
-	  wasteful of CPU.
-
-	  Instead, we only update the gyro_bias vector when we get a
-	  new magnetometer point, and use the much simpler
-	  update_IMU() as the main quaternion update function.
-	 */
-	_gyro_sum += gyro;
-	_accel_sum += accel;
-	_sum_count++;
-
-	if (_compass != NULL && _compass->use_for_yaw() &&
-	    _compass->last_update != _compass_last_update &&
-	    _sum_count != 0) {
-		// use new compass sample for drift correction
-		float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
-		Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
-		_sum_count = 0;
-		update_drift(mag_deltat, mag);
-		_compass_last_update = _compass->last_update;
 	}
 
-	// step the quaternion solution using just gyros and accels
-	gyro -= gyro_bias;
-	update_IMU(deltat, gyro, accel);
-#else
-	Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
-	update_MARG(deltat, gyro, accel, mag);
-#endif
-
 #ifdef DESKTOP_BUILD
 	if (q.is_nan()) {
 		SITL_debug("QUAT NAN: deltat=%f roll=%f pitch=%f yaw=%f q=[%f %f %f %f] a=[%f %f %f] g=(%f %f %f)\n",
@@ -587,7 +384,7 @@ void AP_Quaternion::update(void)
 }
 
 // average error in roll/pitch since last call
-float AP_Quaternion::get_error_rp(void)
+float AP_AHRS_Quaternion::get_error_rp(void)
 {
 	float ret;
 	if (_error_rp_count == 0) {
@@ -600,7 +397,7 @@ float AP_Quaternion::get_error_rp(void)
 }
 
 // average error in yaw since last call
-float AP_Quaternion::get_error_yaw(void)
+float AP_AHRS_Quaternion::get_error_yaw(void)
 {
 	float ret;
 	if (_error_yaw_count == 0) {
@@ -611,3 +408,18 @@ float AP_Quaternion::get_error_yaw(void)
 	_error_yaw_count = 0;
 	return ret;
 }
+
+// reset attitude system
+void AP_AHRS_Quaternion::reset(bool recover_eulers)
+{
+	if (recover_eulers) {
+		q.from_euler(roll, pitch, yaw);
+	} else {
+		q(1, 0, 0, 0);
+	}
+	gyro_bias.zero();
+
+	// reference direction of flux in earth frame
+	b_x = 0;
+	b_z = -1;
+}

libraries/AP_AHRS/AP_AHRS_Quaternion.h

@@ -0,0 +1,94 @@
+#ifndef AP_Quaternion_h
+#define AP_Quaternion_h
+
+#include <AP_Math.h>
+#include <inttypes.h>
+#include <AP_Compass.h>
+#include <AP_GPS.h>
+#include <AP_IMU.h>
+
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "WProgram.h"
+#endif
+
+class AP_AHRS_Quaternion : public AP_AHRS
+{
+public:
+	// Constructor
+	AP_AHRS_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
+	{
+		// scaled gyro drift limits
+		beta = sqrt(3.0f / 4.0f) * gyroMeasError;
+		zeta = sqrt(3.0f / 4.0f) * _gyro_drift_limit;
+
+		// reset attitude
+		reset();
+	}
+
+	// Methods
+	void 		update(void);
+	void 		reset(bool recover_eulers=false);
+
+	// get corrected gyro vector
+	Vector3f get_gyro(void) {
+		// notice the sign reversals here
+		return Vector3f(_gyro_corrected.x, _gyro_corrected.y, _gyro_corrected.z);
+	}
+
+	Vector3f get_gyro_drift(void) {
+		// notice the sign reversals here. The quaternion
+		// system uses a -ve gyro bias, DCM uses a +ve
+		return Vector3f(-gyro_bias.x, -gyro_bias.y, -gyro_bias.z);
+        }
+
+	float get_error_rp(void);
+	float get_error_yaw(void);
+
+	// convert quaternion to a DCM matrix, used by compass
+	// null offsets code
+	Matrix3f get_dcm_matrix(void) {
+		Matrix3f ret;
+		q.rotation_matrix(ret);
+		return ret;
+	}
+
+private:
+	void 		update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
+	void 		update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
+
+	bool		_have_initial_yaw;
+
+	// Methods
+	void 		accel_adjust(void);
+
+	// maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
+	static const float gyroMeasError = 20.0 * (M_PI/180.0);
+
+	// scaled tuning constants
+	float beta;
+	float zeta;
+
+	// quaternion elements
+	Quaternion q;
+
+	// magnetic flux estimates. These are used for the automatic
+	// magnetometer calibration
+	float b_x;
+	float b_z;
+
+	// estimate gyroscope biases error
+	Vector3f gyro_bias;
+
+	// the current corrected gyro vector
+	Vector3f _gyro_corrected;
+
+	// estimate of error
+	float		_error_rp_sum;
+	uint16_t	_error_rp_count;
+	float		_error_yaw_sum;
+	uint16_t	_error_yaw_count;
+};
+
+#endif

libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde

@@ -1,7 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
 //
-// Simple test for the AP_Quaternion library
+// Simple test for the AP_AHRS interface
 //
 
 #include <FastSerial.h>
@@ -13,8 +13,7 @@
 #include <AP_ADC.h>
 #include <AP_IMU.h>
 #include <AP_GPS.h>
-#include <AP_Quaternion.h>
+#include <AP_AHRS.h>
-#include <AP_DCM.h>
 #include <AP_Math.h>
 #include <AP_Common.h>
 #include <AP_Compass.h>
@@ -24,7 +23,7 @@
 #include <GCS_MAVLink.h>
 
 // uncomment this for a APM2 board
-//#define APM2_HARDWARE
+#define APM2_HARDWARE
 
 FastSerialPort(Serial, 0);
 
@@ -42,13 +41,15 @@ AP_Compass_HMC5843 compass;
 # else
   AP_ADC_ADS7844          adc;
   AP_InertialSensor_Oilpan ins( &adc );
-#endif // CONFIG_IMU_TYPE
+#endif
 
 static GPS         *g_gps;
 
 AP_IMU_INS imu(&ins);
-AP_Quaternion  ahrs(&imu, g_gps);
+
-//AP_DCM  ahrs(&imu, g_gps);
+// choose which AHRS system to use
+//AP_AHRS_DCM  ahrs(&imu, g_gps);
+AP_AHRS_Quaternion  ahrs(&imu, g_gps);
 
 AP_Baro_BMP085_HIL      barometer;
 
@@ -122,6 +123,7 @@ void loop(void)
 
 	if (now - last_compass > 100*1000UL &&
 		compass.read()) {
+		compass.calculate(ahrs.get_dcm_matrix());
 		// read compass at 10Hz
 		last_compass = now;
 	}

libraries/AP_DCM/AP_DCM.h

@@ -1,157 +0,0 @@
-#ifndef AP_DCM_h
-#define AP_DCM_h
-
-// temporarily include all other classes here
-// since this naming is a bit off from the
-// convention and the AP_DCM should be the top
-// header file
-#include "AP_DCM_HIL.h"
-
-#include "../FastSerial/FastSerial.h"
-#include "../AP_Math/AP_Math.h"
-#include <inttypes.h>
-#include "../AP_Compass/AP_Compass.h"
-#include "../AP_ADC/AP_ADC.h"
-#include "../AP_GPS/AP_GPS.h"
-#include "../AP_IMU/AP_IMU.h"
-#if defined(ARDUINO) && ARDUINO >= 100
-	#include "Arduino.h"
-#else
-	#include "WProgram.h"
-#endif
-
-class AP_DCM
-{
-public:
-	// Constructors
-	AP_DCM(IMU *imu, GPS *&gps) :
-		_kp_roll_pitch(0.13),
-		_kp_yaw(0.4),
-		_gps(gps),
-		_imu(imu),
-		_dcm_matrix(1, 0, 0,
-			    0, 1, 0,
-			    0, 0, 1),
-		_health(1.),
-		_toggle(0)
-	{
-		// base the ki values by the sensors maximum drift
-		// rate. The APM2 has gyros which are much less drift
-		// prone than the APM1, so we should have a lower ki,
-		// which will make us less prone to increasing omegaI
-		// incorrectly due to sensor noise
-		_gyro_drift_limit = imu->get_gyro_drift_rate();
-		_ki_roll_pitch = _gyro_drift_limit * 5;
-		_ki_yaw        = _gyro_drift_limit * 8;
-	}
-
-	// Accessors
-
-	// return the smoothed gyro vector corrected for drift
-	Vector3f	get_gyro(void) {return _omega_smoothed; }
-	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
-	Matrix3f	get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix;  return temp.transpose();}
-
-	// return the current drift correction integrator value
-	Vector3f	get_gyro_drift(void) {return _omega_I; }
-
-	float		get_health(void) {return _health;}
-	void		set_centripetal(bool b) {_centripetal = b;}
-	bool		get_centripetal(void) {return _centripetal;}
-	void		set_compass(Compass *compass);
-
-	// Methods
-	void 		update_DCM(uint8_t drift_correction_frequency=1);
-	void 		update(void) { update_DCM(); }
-	void 		matrix_reset(bool recover_eulers = false);
-
-	long		roll_sensor;					// Degrees * 100
-	long		pitch_sensor;					// Degrees * 100
-	long		yaw_sensor;					// Degrees * 100
-
-	float		roll;							// Radians
-	float		pitch;							// Radians
-	float		yaw;							// Radians
-
-	uint8_t 	gyro_sat_count;
-	uint8_t 	renorm_range_count;
-	uint8_t 	renorm_blowup_count;
-
-	// status reporting
-	float		get_accel_weight(void);
-	float		get_renorm_val(void);
-	float		get_error_rp(void);
-	float		get_error_yaw(void);
-
-private:
-	float		_kp_roll_pitch;
-	float		_ki_roll_pitch;
-	float		_kp_yaw;
-	float		_ki_yaw;
-	float		_gyro_drift_limit;          // radians/s/s
-	bool		_have_initial_yaw;
-
-	// Methods
-	void 		read_adc_raw(void);
-	void 		accel_adjust(Vector3f &accel);
-	float 		read_adc(int select);
-	void 		matrix_update(float _G_Dt);
-	void 		normalize(void);
-	void		check_matrix(void);
-	bool	 	renorm(Vector3f const &a, Vector3f &result);
-	void 		drift_correction(float deltat);
-	void 		euler_angles(void);
-
-	// members
-	Compass 	* _compass;
-
-	// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
-	//       IMU under us without our noticing.
-	GPS 		*&_gps;                     // note: this is a reference to a pointer owned by the caller
-
-	IMU 		*_imu;
-
-	Matrix3f	_dcm_matrix;
-
-	// sum of accel vectors between drift_correction() calls
-	// this allows the drift correction to run at a different rate
-	// to the main DCM update code
-	Vector3f 	_accel_vector;
-	Vector3f 	_accel_sum;
-
-	Vector3f 	_gyro_vector;		// Store the gyros turn rate in a vector
-	Vector3f	_omega_P;		// accel Omega Proportional correction
-	Vector3f	_omega_yaw_P;		// yaw Omega Proportional correction
-	Vector3f 	_omega_I;		// Omega Integrator correction
-	Vector3f 	_omega_integ_corr;	// Partially corrected Gyro_Vector data - used for centrepetal correction
-	Vector3f 	_omega;			// Corrected Gyro_Vector data
-	Vector3f 	_omega_sum;
-	Vector3f 	_omega_smoothed;
-	float		_health;
-	bool		_centripetal;
-	uint8_t		_toggle;
-
-	// state to support status reporting
-	float		_renorm_val_sum;
-	uint16_t	_renorm_val_count;
-	float		_error_rp_sum;
-	uint16_t	_error_rp_count;
-	float		_error_yaw_sum;
-	uint16_t	_error_yaw_count;
-
-	// time in micros when we last got a compass fix
-	uint32_t	_compass_last_update;
-
-	// time in millis when we last got a GPS heading
-	uint32_t	_gps_last_update;
-
-	// counter of calls to update_DCM() without drift correction
-	uint8_t         _drift_correction_count;
-	float		_drift_correction_time;
-
-};
-
-#endif
-
-
-

libraries/AP_DCM/AP_DCM_HIL.cpp

@@ -1,53 +0,0 @@
-/*
-	APM_DCM_HIL.cpp - DCM AHRS Library, for Ardupilot Mega, Hardware in the Loop Model
-		Code by James Goppert. DIYDrones.com
-
-	This library is free software; you can redistribute it and/or
-    modify it under the terms of the GNU Lesser General Public
-    License as published by the Free Software Foundation; either
-    version 2.1 of the License, or (at your option) any later version.
-
-        Methods:
-				update_DCM()	: Updates the AHRS by integrating the rotation matrix over time using the IMU object data
-				get_gyro()			: Returns gyro vector corrected for bias
-				get_accel()		: Returns accelerometer vector
-				get_dcm_matrix()	: Returns dcm matrix
-
-*/
-#include <AP_DCM_HIL.h>
-
-#define ToRad(x) (x*0.01745329252)	// *pi/180
-#define ToDeg(x) (x*57.2957795131)	// *180/pi
-
-/**************************************************/
-void
-AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
-		float _rollRate, float _pitchRate, float _yawRate)
-{
-	roll = _roll;
-	pitch = _pitch;
-	yaw = _yaw;
-
-	_omega_integ_corr.x = _rollRate;
-	_omega_integ_corr.y = _pitchRate;
-	_omega_integ_corr.z = _yawRate;
-
-	roll_sensor =ToDeg(roll)*100;
-	pitch_sensor =ToDeg(pitch)*100;
-	yaw_sensor =ToDeg(yaw)*100;
-
-	// Need the standard C_body<-nav dcm from navigation frame to body frame
-	// Strapdown Inertial Navigation Technology / Titterton/ pg. 41
-	float sRoll = sin(roll), cRoll = cos(roll);
-	float sPitch = sin(pitch), cPitch = cos(pitch);
-	float sYaw = sin(yaw), cYaw = cos(yaw);
-	_dcm_matrix.a.x =  cPitch*cYaw;
-	_dcm_matrix.a.y =  -cRoll*sYaw+sRoll*sPitch*cYaw;
-	_dcm_matrix.a.z =  sRoll*sYaw+cRoll*sPitch*cYaw;
-	_dcm_matrix.b.x =  cPitch*sYaw;
-	_dcm_matrix.b.y =  cRoll*cYaw+sRoll*sPitch*sYaw;
-	_dcm_matrix.b.z =  -sRoll*cYaw+cRoll*sPitch*sYaw;
-	_dcm_matrix.c.x =  -sPitch;
-	_dcm_matrix.c.y =  sRoll*cPitch;
-	_dcm_matrix.c.z =  cRoll*cPitch;
-}

libraries/AP_DCM/AP_DCM_HIL.h

@@ -1,81 +0,0 @@
-#ifndef AP_DCM_HIL_H
-#define AP_DCM_HIL_H
-
-#include "../FastSerial/FastSerial.h"
-#include "../AP_Math/AP_Math.h"
-#include <inttypes.h>
-#include "../AP_Compass/AP_Compass.h"
-#include "../AP_ADC/AP_ADC.h"
-#include "../AP_GPS/AP_GPS.h"
-#include "../AP_IMU/AP_IMU.h"
-#if defined(ARDUINO) && ARDUINO >= 100
-	#include "Arduino.h"
-#else
-	#include "WProgram.h"
-#endif
-
-
-class AP_DCM_HIL
-{
-public:
-	// Constructors
-	AP_DCM_HIL() :
-		_dcm_matrix(1, 0, 0,
-					0, 1, 0,
-					0, 0, 1)
-	{}
-
-	// Accessors
-	Vector3f	get_gyro(void) {return _omega_integ_corr; }
-	Vector3f	get_accel(void) { return _accel_vector; }
-	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
-	Matrix3f	get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix;  return temp.transpose();}
-
-	void		set_centripetal(bool b) {}
-	void		set_compass(Compass *compass) {}
-
-	// Methods
-	void 		update_DCM(void) {}
-	void 		update_DCM_fast(void) {}
-
-	float		get_health(void) { return 1.0; }
-
-	long		roll_sensor;					// Degrees * 100
-	long		pitch_sensor;					// Degrees * 100
-	long		yaw_sensor;						// Degrees * 100
-
-	float		roll;							// Radians
-	float		pitch;							// Radians
-	float		yaw;							// Radians
-
-	uint8_t 	gyro_sat_count;
-	uint8_t 	renorm_range_count;
-	uint8_t 	renorm_blowup_count;
-
-
-	float	kp_roll_pitch() 		{ return 0; }
-	void	kp_roll_pitch(float v) 	{  }
-
-	float	ki_roll_pitch() 		{ return 0; }
-	void	ki_roll_pitch(float v) 	{  }
-
-	float	kp_yaw() 				{ return 0; }
-	void	kp_yaw(float v) 		{  }
-
-	float	ki_yaw() 				{ return 0; }
-	void	ki_yaw(float v) 		{  }
-
-
-	void 		setHil(float roll, float pitch, float yaw,
-					float rollRate, float pitchRate, float yawRate);
-private:
-	// Methods
-	Matrix3f	_dcm_matrix;
-	Vector3f 	_omega_integ_corr;
-	Vector3f 	_accel_vector;
-};
-
-#endif
-
-
-

libraries/AP_Math/AP_Math.cpp

@@ -30,3 +30,40 @@ float safe_sqrt(float v)
 	}
 	return ret;
 }
+
+
+// find a rotation that is the combination of two other
+// rotations. This is used to allow us to add an overall board
+// rotation to an existing rotation of a sensor such as the compass
+// Note that this relies the set of rotations being complete. The
+// optional 'found' parameter is for the test suite to ensure that it is.
+enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found)
+{
+	Vector3f tv1, tv2;
+	enum Rotation r;
+	tv1(1,2,3);
+	tv1.rotate(r1);
+	tv1.rotate(r2);
+
+	for (r=ROTATION_NONE; r<ROTATION_MAX;
+	     r = (enum Rotation)((uint8_t)r+1)) {
+		Vector3f diff;
+		tv2(1,2,3);
+		tv2.rotate(r);
+		diff = tv1 - tv2;
+		if (diff.length() < 1.0e-6) {
+			// we found a match
+			if (found) {
+				*found = true;
+			}
+			return r;
+		}
+	}
+
+	// we found no matching rotation. Someone has edited the
+	// rotations list and broken its completeness property ...
+	if (found) {
+		*found = false;
+	}
+	return ROTATION_NONE;
+}

libraries/AP_Math/AP_Math.h

@@ -24,4 +24,9 @@ float safe_asin(float v);
 // a varient of sqrt() that always gives a valid answer.
 float safe_sqrt(float v);
 
+// find a rotation that is the combination of two other
+// rotations. This is used to allow us to add an overall board
+// rotation to an existing rotation of a sensor such as the compass
+enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);
+
 #endif

libraries/AP_Math/examples/eulers/eulers.pde

@@ -17,6 +17,8 @@ FastSerialPort(Serial, 0);
 #include <Arduino_Mega_ISR_Registry.h>
 #include <AP_PeriodicProcess.h>
 #include <AP_ADC.h>
+#include <SPI.h>
+#include <I2C.h>
 #include <AP_Baro.h>
 #include <AP_Compass.h>
 #include <AP_GPS.h>
@@ -25,6 +27,10 @@ AP_Baro_BMP085_HIL      barometer;
 AP_Compass_HIL     compass;
 #endif
 
+#if 0
+#include <AP_Declination.h>
+#endif
+
 static float rad_diff(float rad1, float rad2)
 {
     float diff = rad1 - rad2;
@@ -206,6 +212,52 @@ void test_frame_transforms(void)
     printf("%f %f %f\n", v2.x, v2.y, v2.z);
 }
 
+// generate a random float between -1 and 1
+static float rand_num(void)
+{
+	float ret = ((unsigned)random()) % 2000000;
+	return (ret - 1.0e6) / 1.0e6;
+}
+
+void test_matrix_rotate(void)
+{
+    Matrix3f m1, m2, diff;
+    Vector3f r;
+
+    m1.identity();
+    m2.identity();
+    r.x = rand_num();
+    r.y = rand_num();
+    r.z = rand_num();
+
+    for (uint16_t i = 0; i<1000; i++) {
+        // old method
+        Matrix3f temp_matrix;
+        temp_matrix.a.x = 0;
+        temp_matrix.a.y = -r.z;
+        temp_matrix.a.z =  r.y;
+        temp_matrix.b.x =  r.z;
+        temp_matrix.b.y = 0;
+        temp_matrix.b.z = -r.x;
+        temp_matrix.c.x = -r.y;
+        temp_matrix.c.y =  r.x;
+        temp_matrix.c.z = 0;
+        temp_matrix = m1 * temp_matrix;
+        m1 += temp_matrix;
+
+        // new method
+        m2.rotate(r);
+
+        // check they behave in the same way
+        diff = m1 - m2;
+        float err = diff.a.length() + diff.b.length() + diff.c.length();
+
+        if (err > 0) {
+            Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err);
+        }
+    }
+}
+
 /*
   euler angle tests
  */
@@ -220,6 +272,7 @@ void setup(void)
     test_conversions();
     test_quaternion_eulers();
     test_matrix_eulers();
+    test_matrix_rotate();
 }
 
 void

libraries/AP_Math/examples/rotations/rotations.pde

@@ -25,6 +25,11 @@ AP_Baro_BMP085_HIL      barometer;
 AP_Compass_HIL     compass;
 #endif
 
+#if AUTOMATIC_DECLINATION == ENABLED
+// this is in an #if to avoid the static data
+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#endif
+
 
 // standard rotation matrices (these are the originals from the old code)
 #define MATRIX_ROTATION_NONE               Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
@@ -162,6 +167,28 @@ static void test_vectors(void)
 }
 
 
+// test combinations of rotations
+static void test_combinations(void)
+{
+    enum Rotation r1, r2, r3;
+    bool found;
+
+	for (r1=ROTATION_NONE; r1<ROTATION_MAX;
+	     r1 = (enum Rotation)((uint8_t)r1+1)) {
+        for (r2=ROTATION_NONE; r2<ROTATION_MAX;
+             r2 = (enum Rotation)((uint8_t)r2+1)) {
+            r3 = rotation_combination(r1, r2, &found);
+            if (found) {
+                Serial.printf("rotation: %u + %u -> %u\n",
+                              (unsigned)r1, (unsigned)r2, (unsigned)r3);
+            } else {
+                Serial.printf("ERROR rotation: no combination for %u + %u\n",
+                              (unsigned)r1, (unsigned)r2);
+            }
+        }
+    }
+}
+
 /*
   rotation tests
  */
@@ -171,6 +198,7 @@ void setup(void)
     Serial.println("rotation unit tests\n");
     test_matrices();
     test_vectors();
+    test_combinations();
     Serial.println("rotation unit tests done\n");
 }
 

libraries/AP_Math/matrix3.cpp

@@ -44,6 +44,7 @@ void Matrix3<T>::rotation(enum Rotation r)
 {
     switch (r) {
     case ROTATION_NONE:
+    case ROTATION_MAX:
 	    *this = MATRIX_ROTATION_NONE;
 	    break;
     case ROTATION_YAW_45:
@@ -133,7 +134,27 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
 	}
 }
 
+// apply an additional rotation from a body frame gyro vector
+// to a rotation matrix.
+template <typename T>
+void Matrix3<T>::rotate(const Vector3<T> &g)
+{
+	Matrix3f temp_matrix;
+	temp_matrix.a.x = a.y * g.z - a.z * g.y;
+	temp_matrix.a.y = a.z * g.x - a.x * g.z;
+	temp_matrix.a.z = a.x * g.y - a.y * g.x;
+	temp_matrix.b.x = b.y * g.z - b.z * g.y;
+	temp_matrix.b.y = b.z * g.x - b.x * g.z;
+	temp_matrix.b.z = b.x * g.y - b.y * g.x;
+	temp_matrix.c.x = c.y * g.z - c.z * g.y;
+	temp_matrix.c.y = c.z * g.x - c.x * g.z;
+	temp_matrix.c.z = c.x * g.y - c.y * g.x;
+
+	(*this) += temp_matrix;
+}
+
 // only define for float
 template void Matrix3<float>::rotation(enum Rotation);
+template void Matrix3<float>::rotate(const Vector3<float> &g);
 template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
 template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);

libraries/AP_Math/matrix3.h

@@ -124,6 +124,21 @@ public:
 	Matrix3<T> transpose(void)
 	{	return *this = transposed();	}
 
+	// zero the matrix
+	void zero(void) {
+		a.x = a.y = a.z = 0;
+		b.x = b.y = b.z = 0;
+		c.x = c.y = c.z = 0;
+	}
+
+	// setup the identity matrix
+	void identity(void) {
+		a.x = b.y = c.z = 1;
+		a.y = a.z = 0;
+		b.x = b.z = 0;
+		c.x = c.y = 0;
+	}
+
 	// check if any elements are NAN
 	bool is_nan(void)
 		{   return a.is_nan() || b.is_nan() || c.is_nan(); }
@@ -136,6 +151,10 @@ public:
 
     // create eulers from a rotation matrix
 	void to_euler(float *roll, float *pitch, float *yaw);
+
+    // apply an additional rotation from a body frame gyro vector
+    // to a rotation matrix.
+	void rotate(const Vector3<T> &g);
 };
 
 typedef Matrix3<int16_t>		Matrix3i;

libraries/AP_Math/quaternion.cpp

@@ -34,12 +34,12 @@ void Quaternion::rotation_matrix(Matrix3f &m)
 
     m.a.x = 1-2*(q3q3 + q4q4);
     m.a.y =   2*(q2q3 - q1q4);
-    m.a.z = - 2*(q2q4 + q1q3);
+    m.a.z =   2*(q2q4 + q1q3);
     m.b.x =   2*(q2q3 + q1q4);
     m.b.y = 1-2*(q2q2 + q4q4);
-    m.b.z =  -2*(q3q4 - q1q2);
+    m.b.z =   2*(q3q4 - q1q2);
-    m.c.x =  -2*(q2q4 - q1q3);
+    m.c.x =   2*(q2q4 - q1q3);
-    m.c.y =  -2*(q3q4 + q1q2);
+    m.c.y =   2*(q3q4 + q1q2);
     m.c.z = 1-2*(q2q2 + q3q3);
 }
 
@@ -61,10 +61,8 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
     float cr2 = cos(roll*0.5);
     float cp2 = cos(pitch*0.5);
     float cy2 = cos(yaw*0.5);
-    // the sign reversal here is due to the different conventions
+    float sr2 = sin(roll*0.5);
-    // in the madgwick quaternion code and the rest of APM
+    float sp2 = sin(pitch*0.5);
-    float sr2 = -sin(roll*0.5);
-    float sp2 = -sin(pitch*0.5);
     float sy2 = sin(yaw*0.5);
 
     q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
@@ -77,12 +75,12 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
 void Quaternion::to_euler(float *roll, float *pitch, float *yaw)
 {
     if (roll) {
-        *roll = -(atan2(2.0*(q1*q2 + q3*q4),
+        *roll = (atan2(2.0*(q1*q2 + q3*q4),
-                        1 - 2.0*(q2*q2 + q3*q3)));
+                       1 - 2.0*(q2*q2 + q3*q3)));
     }
     if (pitch) {
         // we let safe_asin() handle the singularities near 90/-90 in pitch
-        *pitch = -safe_asin(2.0*(q1*q3 - q4*q2));
+        *pitch = safe_asin(2.0*(q1*q3 - q4*q2));
     }
     if (yaw) {
         *yaw = atan2(2.0*(q1*q4 + q2*q3),

libraries/AP_Math/rotations.h

@@ -17,6 +17,12 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+
+// these rotations form a full set - every rotation in the following
+// list when combined with another in the list forms an entry which is
+// also in the list. This is an important property. Please run the
+// rotations test suite if you add to the list.
+
 // these rotation values are stored to EEPROM, so be careful not to
 // change the numbering of any existing entry when adding a new entry.
 enum Rotation {
@@ -35,5 +41,6 @@ enum Rotation {
 	ROTATION_PITCH_180,
 	ROTATION_ROLL_180_YAW_225,
 	ROTATION_ROLL_180_YAW_270,
-	ROTATION_ROLL_180_YAW_315
+	ROTATION_ROLL_180_YAW_315,
+    ROTATION_MAX
 };

libraries/AP_Math/vector3.cpp

@@ -29,6 +29,7 @@ void Vector3<T>::rotate(enum Rotation rotation)
     T tmp;
     switch (rotation) {
     case ROTATION_NONE:
+    case ROTATION_MAX:
         return;
     case ROTATION_YAW_45: {
         tmp = HALF_SQRT_2*(x - y);

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;
+	_ahrs=ahrs;
-	_dcm_hil=NULL;
 	_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)
+		if (_ahrs)
 		{
-			roll_angle  = -_dcm->roll_sensor;
+			roll_angle  = -_ahrs->roll_sensor;
-			pitch_angle = -_dcm->pitch_sensor;
+			pitch_angle = -_ahrs->pitch_sensor;
-			yaw_angle   = -_dcm->yaw_sensor;
+			yaw_angle   = -_ahrs->yaw_sensor;
-		}
-		if (_dcm_hil)
-		{
-			roll_angle  = -_dcm_hil->roll_sensor;
-			pitch_angle = -_dcm_hil->pitch_sensor;
-			yaw_angle   = -_dcm_hil->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_AHRS *ahrs);
-	AP_Mount(GPS *gps, AP_DCM_HIL *dcm); // constructor for HIL usage
 
 	//enums
 	enum MountType{
@@ -72,8 +71,7 @@ private:
 	long rc_map(RC_Channel_aux* rc_ch);
 
 	//members
-	AP_DCM		*_dcm;
+	AP_AHRS		*_ahrs;
-	AP_DCM_HIL  *_dcm_hil;
 	GPS 		*_gps;
 
 	int roll_angle;		///< degrees*100

libraries/AP_Quaternion/AP_Quaternion.h

@@ -1,139 +0,0 @@
-#ifndef AP_Quaternion_h
-#define AP_Quaternion_h
-
-#include <AP_Math.h>
-#include <inttypes.h>
-#include <AP_Compass.h>
-#include <AP_GPS.h>
-#include <AP_IMU.h>
-
-#if defined(ARDUINO) && ARDUINO >= 100
-	#include "Arduino.h"
-#else
-	#include "WProgram.h"
-#endif
-
-class AP_Quaternion
-{
-public:
-	// Constructor
-	AP_Quaternion(IMU *imu, GPS *&gps) :
-		_imu(imu),
-		_gps(gps)
-	{
-		// reference direction of flux in earth frame
-		b_x = 0;
-		b_z = -1;
-
-		// limit the drift to the drift rate reported by the
-		// sensor driver
-		gyroMeasDrift = imu->get_gyro_drift_rate();
-
-		// scaled gyro drift limits
-		beta = sqrt(3.0f / 4.0f) * gyroMeasError;
-		zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;
-	}
-
-	// Accessors
-	void		set_centripetal(bool b) {_centripetal = b;}
-	bool		get_centripetal(void) {return _centripetal;}
-	void		set_compass(Compass *compass);
-
-	// Methods
-	void 		update(void);
-
-	// Euler angles (radians)
-	float		roll;
-	float		pitch;
-	float		yaw;
-
-	// integer Euler angles (Degrees * 100)
-	int32_t		roll_sensor;
-	int32_t		pitch_sensor;
-	int32_t		yaw_sensor;
-
-
-	// compatibility methods with DCM
-	void update_DCM(void) { update(); }
-	void update_DCM_fast(void) { update(); }
-	Vector3f get_gyro(void) {
-		// notice the sign reversals here
-		return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
-	}
-	Vector3f get_gyro_drift(void) {
-		// notice the sign reversals here
-		return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
-        }
-	float get_accel_weight(void) { return 0; }
-	float get_renorm_val(void) { return 0; }
-	float get_health(void) { return 0; }
-	void matrix_reset(void) { }
-	uint8_t gyro_sat_count;
-	uint8_t renorm_range_count;
-	uint8_t renorm_blowup_count;
-	float get_error_rp(void);
-	float get_error_yaw(void);
-	Matrix3f get_dcm_matrix(void) {
-		Matrix3f ret;
-		q.rotation_matrix(ret);
-		return ret;
-	}
-
-private:
-	void 		update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
-	void 		update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
-	void 		update_drift(float deltat, Vector3f &mag);
-
-	bool		_have_initial_yaw;
-
-	// Methods
-	void 		accel_adjust(void);
-
-	// members
-	Compass 	* _compass;
-	// time in microseconds of last compass update
-	uint32_t        _compass_last_update;
-
-	// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
-	//       IMU under us without our noticing.
-	GPS 		*&_gps;
-	IMU 		*_imu;
-
-	// true if we are doing centripetal acceleration correction
-	bool		_centripetal;
-
-	// maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
-	static const float gyroMeasError = 20.0 * (M_PI/180.0);
-
-	float gyroMeasDrift;
-
-	float beta;
-	float zeta;
-
-	// quaternion elements
-	Quaternion q;
-
-	// magnetic flux estimates. These are used for the automatic
-	// magnetometer calibration
-	float b_x;
-	float b_z;
-
-	// estimate gyroscope biases error
-	Vector3f gyro_bias;
-
-	// the current corrected gyro vector
-	Vector3f _gyro_corrected;
-
-	// accel and gyro accumulators for drift correction
-	Vector3f _gyro_sum;
-	Vector3f _accel_sum;
-	uint32_t _sum_count;
-
-	// estimate of error
-	float		_error_rp_sum;
-	uint16_t	_error_rp_count;
-	float		_error_yaw_sum;
-	uint16_t	_error_yaw_count;
-};
-
-#endif

libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h

@@ -16,11 +16,11 @@ extern "C" {
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 205, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}
+#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_DCM, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG}
 #endif
 
 #include "../protocol.h"
@@ -143,7 +143,7 @@ enum FENCE_BREACH
 #include "./mavlink_msg_fence_point.h"
 #include "./mavlink_msg_fence_fetch_point.h"
 #include "./mavlink_msg_fence_status.h"
-#include "./mavlink_msg_dcm.h"
+#include "./mavlink_msg_ahrs.h"
 #include "./mavlink_msg_simstate.h"
 #include "./mavlink_msg_hwstatus.h"
 

libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ahrs.h

@@ -0,0 +1,276 @@
+// MESSAGE AHRS PACKING
+
+#define MAVLINK_MSG_ID_AHRS 163
+
+typedef struct __mavlink_ahrs_t
+{
+ float omegaIx; ///< X gyro drift estimate rad/s
+ float omegaIy; ///< Y gyro drift estimate rad/s
+ float omegaIz; ///< Z gyro drift estimate rad/s
+ float accel_weight; ///< average accel_weight
+ float renorm_val; ///< average renormalisation value
+ float error_rp; ///< average error_roll_pitch value
+ float error_yaw; ///< average error_yaw value
+} mavlink_ahrs_t;
+
+#define MAVLINK_MSG_ID_AHRS_LEN 28
+#define MAVLINK_MSG_ID_163_LEN 28
+
+
+
+#define MAVLINK_MESSAGE_INFO_AHRS { \
+	"AHRS", \
+	7, \
+	{  { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
+         { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
+         { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
+         { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
+         { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
+         { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
+         { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a ahrs message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[28];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+        memcpy(_MAV_PAYLOAD(msg), buf, 28);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+        memcpy(_MAV_PAYLOAD(msg), &packet, 28);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_AHRS;
+	return mavlink_finalize_message(msg, system_id, component_id, 28);
+}
+
+/**
+ * @brief Pack a ahrs message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[28];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+        memcpy(_MAV_PAYLOAD(msg), buf, 28);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+        memcpy(_MAV_PAYLOAD(msg), &packet, 28);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_AHRS;
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28);
+}
+
+/**
+ * @brief Encode a ahrs struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param ahrs C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
+{
+	return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
+}
+
+/**
+ * @brief Send a ahrs message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[28];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28);
+#endif
+}
+
+#endif
+
+// MESSAGE AHRS UNPACKING
+
+
+/**
+ * @brief Get field omegaIx from ahrs message
+ *
+ * @return X gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  0);
+}
+
+/**
+ * @brief Get field omegaIy from ahrs message
+ *
+ * @return Y gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  4);
+}
+
+/**
+ * @brief Get field omegaIz from ahrs message
+ *
+ * @return Z gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  8);
+}
+
+/**
+ * @brief Get field accel_weight from ahrs message
+ *
+ * @return average accel_weight
+ */
+static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  12);
+}
+
+/**
+ * @brief Get field renorm_val from ahrs message
+ *
+ * @return average renormalisation value
+ */
+static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  16);
+}
+
+/**
+ * @brief Get field error_rp from ahrs message
+ *
+ * @return average error_roll_pitch value
+ */
+static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  20);
+}
+
+/**
+ * @brief Get field error_yaw from ahrs message
+ *
+ * @return average error_yaw value
+ */
+static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  24);
+}
+
+/**
+ * @brief Decode a ahrs message into a struct
+ *
+ * @param msg The message to decode
+ * @param ahrs C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
+	ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
+	ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
+	ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
+	ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
+	ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
+	ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
+#else
+	memcpy(ahrs, _MAV_PAYLOAD(msg), 28);
+#endif
+}

libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h

@@ -668,12 +668,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
-static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
         uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
         uint16_t i;
-	mavlink_dcm_t packet_in = {
+	mavlink_ahrs_t packet_in = {
 		17.0,
 	45.0,
 	73.0,
@@ -682,7 +682,7 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
 	157.0,
 	185.0,
 	};
-	mavlink_dcm_t packet1, packet2;
+	mavlink_ahrs_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
 		packet1.omegaIx = packet_in.omegaIx;
 		packet1.omegaIy = packet_in.omegaIy;
@@ -695,18 +695,18 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
 
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_dcm_decode(&msg, &packet2);
+	mavlink_msg_ahrs_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
+	mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_dcm_decode(&msg, &packet2);
+	mavlink_msg_ahrs_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
+	mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_dcm_decode(&msg, &packet2);
+	mavlink_msg_ahrs_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
@@ -714,12 +714,12 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
         for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
 		comm_send_ch(MAVLINK_COMM_0, buffer[i]);
         }
-	mavlink_msg_dcm_decode(last_msg, &packet2);
+	mavlink_msg_ahrs_decode(last_msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
+	mavlink_msg_ahrs_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_dcm_decode(last_msg, &packet2);
+	mavlink_msg_ahrs_decode(last_msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
@@ -841,7 +841,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
 	mavlink_test_fence_point(system_id, component_id, last_msg);
 	mavlink_test_fence_fetch_point(system_id, component_id, last_msg);
 	mavlink_test_fence_status(system_id, component_id, last_msg);
-	mavlink_test_dcm(system_id, component_id, last_msg);
+	mavlink_test_ahrs(system_id, component_id, last_msg);
 	mavlink_test_simstate(system_id, component_id, last_msg);
 	mavlink_test_hwstatus(system_id, component_id, last_msg);
 }

libraries/GCS_MAVLink/include/ardupilotmega/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:12 2012"
+#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

libraries/GCS_MAVLink/include/common/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:12 2012"
+#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

libraries/GCS_MAVLink/include_v1.0/ardupilotmega/ardupilotmega.h

@@ -16,11 +16,11 @@ extern "C" {
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 205, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, {NULL}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_DCM, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, {NULL}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE}
 #endif
 
 #include "../protocol.h"
@@ -142,7 +142,7 @@ enum FENCE_BREACH
 #include "./mavlink_msg_fence_point.h"
 #include "./mavlink_msg_fence_fetch_point.h"
 #include "./mavlink_msg_fence_status.h"
-#include "./mavlink_msg_dcm.h"
+#include "./mavlink_msg_ahrs.h"
 #include "./mavlink_msg_simstate.h"
 #include "./mavlink_msg_hwstatus.h"
 

libraries/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_ahrs.h

@@ -0,0 +1,276 @@
+// MESSAGE AHRS PACKING
+
+#define MAVLINK_MSG_ID_AHRS 163
+
+typedef struct __mavlink_ahrs_t
+{
+ float omegaIx; ///< X gyro drift estimate rad/s
+ float omegaIy; ///< Y gyro drift estimate rad/s
+ float omegaIz; ///< Z gyro drift estimate rad/s
+ float accel_weight; ///< average accel_weight
+ float renorm_val; ///< average renormalisation value
+ float error_rp; ///< average error_roll_pitch value
+ float error_yaw; ///< average error_yaw value
+} mavlink_ahrs_t;
+
+#define MAVLINK_MSG_ID_AHRS_LEN 28
+#define MAVLINK_MSG_ID_163_LEN 28
+
+
+
+#define MAVLINK_MESSAGE_INFO_AHRS { \
+	"AHRS", \
+	7, \
+	{  { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
+         { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
+         { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
+         { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
+         { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
+         { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
+         { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a ahrs message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[28];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+        memcpy(_MAV_PAYLOAD(msg), buf, 28);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+        memcpy(_MAV_PAYLOAD(msg), &packet, 28);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_AHRS;
+	return mavlink_finalize_message(msg, system_id, component_id, 28, 127);
+}
+
+/**
+ * @brief Pack a ahrs message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[28];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+        memcpy(_MAV_PAYLOAD(msg), buf, 28);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+        memcpy(_MAV_PAYLOAD(msg), &packet, 28);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_AHRS;
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 127);
+}
+
+/**
+ * @brief Encode a ahrs struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param ahrs C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
+{
+	return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
+}
+
+/**
+ * @brief Send a ahrs message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[28];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28, 127);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28, 127);
+#endif
+}
+
+#endif
+
+// MESSAGE AHRS UNPACKING
+
+
+/**
+ * @brief Get field omegaIx from ahrs message
+ *
+ * @return X gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  0);
+}
+
+/**
+ * @brief Get field omegaIy from ahrs message
+ *
+ * @return Y gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  4);
+}
+
+/**
+ * @brief Get field omegaIz from ahrs message
+ *
+ * @return Z gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  8);
+}
+
+/**
+ * @brief Get field accel_weight from ahrs message
+ *
+ * @return average accel_weight
+ */
+static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  12);
+}
+
+/**
+ * @brief Get field renorm_val from ahrs message
+ *
+ * @return average renormalisation value
+ */
+static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  16);
+}
+
+/**
+ * @brief Get field error_rp from ahrs message
+ *
+ * @return average error_roll_pitch value
+ */
+static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  20);
+}
+
+/**
+ * @brief Get field error_yaw from ahrs message
+ *
+ * @return average error_yaw value
+ */
+static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  24);
+}
+
+/**
+ * @brief Decode a ahrs message into a struct
+ *
+ * @param msg The message to decode
+ * @param ahrs C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
+	ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
+	ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
+	ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
+	ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
+	ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
+	ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
+#else
+	memcpy(ahrs, _MAV_PAYLOAD(msg), 28);
+#endif
+}

libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h

@@ -668,12 +668,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
-static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
         uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
         uint16_t i;
-	mavlink_dcm_t packet_in = {
+	mavlink_ahrs_t packet_in = {
 		17.0,
 	45.0,
 	73.0,
@@ -682,7 +682,7 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
 	157.0,
 	185.0,
 	};
-	mavlink_dcm_t packet1, packet2;
+	mavlink_ahrs_t packet1, packet2;
         memset(&packet1, 0, sizeof(packet1));
 		packet1.omegaIx = packet_in.omegaIx;
 		packet1.omegaIy = packet_in.omegaIy;
@@ -695,18 +695,18 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
 
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1);
-	mavlink_msg_dcm_decode(&msg, &packet2);
+	mavlink_msg_ahrs_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
+	mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_dcm_decode(&msg, &packet2);
+	mavlink_msg_ahrs_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
+	mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_dcm_decode(&msg, &packet2);
+	mavlink_msg_ahrs_decode(&msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
@@ -714,12 +714,12 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me
         for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
 		comm_send_ch(MAVLINK_COMM_0, buffer[i]);
         }
-	mavlink_msg_dcm_decode(last_msg, &packet2);
+	mavlink_msg_ahrs_decode(last_msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 
         memset(&packet2, 0, sizeof(packet2));
-	mavlink_msg_dcm_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
+	mavlink_msg_ahrs_send(MAVLINK_COMM_1 , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw );
-	mavlink_msg_dcm_decode(last_msg, &packet2);
+	mavlink_msg_ahrs_decode(last_msg, &packet2);
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
@@ -841,7 +841,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
 	mavlink_test_fence_point(system_id, component_id, last_msg);
 	mavlink_test_fence_fetch_point(system_id, component_id, last_msg);
 	mavlink_test_fence_status(system_id, component_id, last_msg);
-	mavlink_test_dcm(system_id, component_id, last_msg);
+	mavlink_test_ahrs(system_id, component_id, last_msg);
 	mavlink_test_simstate(system_id, component_id, last_msg);
 	mavlink_test_hwstatus(system_id, component_id, last_msg);
 }

libraries/GCS_MAVLink/include_v1.0/ardupilotmega/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:13 2012"
+#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

libraries/GCS_MAVLink/include_v1.0/common/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Sat Mar 10 09:38:13 2012"
+#define MAVLINK_BUILD_DATE "Sun Mar 11 19:38:21 2012"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
  

libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml

@@ -226,7 +226,7 @@
 	    <field name="breach_time" type="uint32_t">time of last breach in milliseconds since boot</field>
 	  </message>
 
-	  <message name="DCM" id="163">
+	  <message name="AHRS" id="163">
 	    <description>Status of DCM attitude estimator</description>
             <field type="float" name="omegaIx">X gyro drift estimate rad/s</field>
             <field type="float" name="omegaIy">Y gyro drift estimate rad/s</field>

libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml

@@ -226,7 +226,7 @@
 	    <field name="breach_time" type="uint32_t">time of last breach in milliseconds since boot</field>
 	  </message>
 
-	  <message name="DCM" id="163">
+	  <message name="AHRS" id="163">
 	    <description>Status of DCM attitude estimator</description>
             <field type="float" name="omegaIx">X gyro drift estimate rad/s</field>
             <field type="float" name="omegaIy">Y gyro drift estimate rad/s</field>