ACM: make it possible to build ArduCopter with quaternions

ArduCopter/ArduCopter.pde

@@ -72,6 +72,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
 #include <AP_PeriodicProcess.h>         // Parent header of Timer
                                         // (only included for makefile libpath to work)
 #include <AP_TimerProcess.h>            // TimerProcess is the scheduler for MPU6000 reads.
+#include <AP_Quaternion.h>  // Madgwick quaternion system
 #include <AP_DCM.h>         // ArduPilot Mega DCM Library
 #include <APM_PI.h>            	// PI library
 #include <AC_PID.h>            // PID library
@@ -222,10 +223,20 @@ AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
 AP_InertialSensor_Oilpan ins(&adc);
 #endif
 AP_IMU_INS  imu(&ins);
+
 // we don't want to use gps for yaw correction on ArduCopter, so pass
 // a NULL GPS object pointer
 static GPS         *g_gps_null;
+
+#if QUATERNION_ENABLE == ENABLED
+  // this shouldn't be called dcm of course, but until we
+  // decide to actually use something else, I don't want the patch
+  // size to be huge
+ AP_Quaternion dcm(&imu, g_gps_null);
+#else
  AP_DCM  dcm(&imu, g_gps_null);
+#endif
+
 AP_TimerProcess timer_scheduler;
 
 #elif HIL_MODE == HIL_MODE_SENSORS
@@ -842,6 +853,7 @@ void setup() {
 void loop()
 {
 	uint32_t timer 			= micros();
+
 	// We want this to execute fast
 	// ----------------------------
 	if ((timer - fast_loopTimer) >= 4000 && imu.new_data_available()) {
@@ -960,8 +972,13 @@ static void medium_loop()
 			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
 				if(g.compass_enabled){
 					if (compass.read()) {
-                        compass.calculate(dcm.get_dcm_matrix());  	// Calculate heading
+                        // Calculate heading
+#if QUATERNION_ENABLE == ENABLED
+                        compass.calculate(dcm.roll, dcm.pitch);
+#else
+                        compass.calculate(dcm.get_dcm_matrix());
                         compass.null_offsets(dcm.get_dcm_matrix());
+#endif
                     }
 				}
 			#endif
@@ -1844,6 +1861,7 @@ static void read_AHRS(void)
 }
 
 static void update_trig(void){
+#if QUATERNION_ENABLE == DISABLED
 	Vector2f yawvector;
 	Matrix3f temp 	= dcm.get_dcm_matrix();
 
@@ -1862,6 +1880,23 @@ static void update_trig(void){
 	sin_yaw_y 		= yawvector.x;						// 1y = north
 	cos_yaw_x 		= yawvector.y;						// 0x = north
 
+#else
+    // we need a cheaper way to calculate this ....
+	Vector2f yawvector;
+	float cp = cos(dcm.pitch);
+	float cr = cos(dcm.roll);
+	float sy = sin(dcm.yaw);
+	float cy = cos(dcm.yaw);
+
+	yawvector.x 	= cp * cy;
+	yawvector.y 	= cp * sy;
+	yawvector.normalize();
+
+	cos_pitch_x 	= cp;
+    cos_roll_x 		= cr;
+	sin_yaw_y 		= yawvector.x;
+	cos_yaw_x 		= yawvector.y;
+#endif
 	//flat:
 	// 0 ° = cos_yaw:  0.00, sin_yaw:  1.00,
 	// 90° = cos_yaw:  1.00, sin_yaw:  0.00,

ArduCopter/GCS_Mavlink.pde

@@ -99,6 +99,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
 
 static void NOINLINE send_location(mavlink_channel_t chan)
 {
+#if QUATERNION_ENABLE == DISABLED
     Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
     mavlink_msg_global_position_int_send(
         chan,
@@ -108,6 +109,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
         g_gps->ground_speed * rot.a.x,
         g_gps->ground_speed * rot.b.x,
         g_gps->ground_speed * rot.c.x);
+#endif
 }
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)

ArduCopter/Makefile

@@ -42,6 +42,9 @@ sitl-hexa:
 sitl-y6:
 	make -f ../libraries/Desktop/Makefile.desktop y6
 
+sitl-quaternion:
+	make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DQUATERNION_ENABLE=ENABLED"
+
 etags:
 	cd .. && etags -f ArduCopter/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduCopter libraries)
 

ArduCopter/config.h

@@ -939,5 +939,9 @@
 #endif
 
 
+// experimental quaternion code
+#ifndef QUATERNION_ENABLE
+# define QUATERNION_ENABLE DISABLED
+#endif
 
 #endif // __ARDUCOPTER_CONFIG_H__

ArduCopter/system.pde

@@ -309,11 +309,13 @@ static void init_ardupilot()
 	reset_control_switch();
 
 	#if HIL_MODE != HIL_MODE_ATTITUDE
+#if QUATERNION_ENABLE == DISABLED
 		dcm.kp_roll_pitch(0.130000);
 		dcm.ki_roll_pitch(0.00001278),	// 50 hz I term
 		dcm.kp_yaw(0.12);
 		dcm.ki_yaw(0.00002);
 		dcm._clamp = 5;
+#endif
 	#endif
 
 	// init the Z damopener

ArduCopter/test.pde

@@ -330,8 +330,12 @@ test_radio(uint8_t argc, const Menu::arg *argv)
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
 					compass.read();		 				// Read magnetometer
-					compass.calculate(dcm.roll, dcm.pitch);		// Calculate heading
+#if QUATERNION_ENABLE == ENABLED
+					compass.calculate(dcm.roll, dcm.pitch);
+#else
+					compass.calculate(dcm.get_dcm_matrix());
                     compass.null_offsets(dcm.get_dcm_matrix());
+#endif
 					medium_loopCounter = 0;
 				}
 			}
@@ -548,7 +552,12 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 
 				if(g.compass_enabled){
 					compass.read();		 				// Read magnetometer
+#if QUATERNION_ENABLE == ENABLED
+					compass.calculate(dcm.roll, dcm.pitch);
+#else
 					compass.calculate(dcm.get_dcm_matrix());
+                    compass.null_offsets(dcm.get_dcm_matrix());
+#endif
 				}
 			}
 			fast_loopTimer = millis();
@@ -932,7 +941,11 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 			while(1){
 				delay(100);
 				if (compass.read()) {
+#if QUATERNION_ENABLE == ENABLED
+					compass.calculate(dcm.roll, dcm.pitch);
+#else
 					compass.calculate(dcm.get_dcm_matrix());
+#endif
 					Vector3f maggy = compass.get_offsets();
 					Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
 									(wrap_360(ToDeg(compass.heading) * 100)) /100,