APM: make it possible to build ArduPlane with quaternion support

ArduPlane/ArduPlane.pde

@@ -41,6 +41,7 @@ version 2.1 of the License, or (at your option) any later version.
 #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_Quaternion.h>  // Madgwick quaternion system
 #include <PID.h>            // PID library
 #include <RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder.h>	// Range finder library
@@ -186,7 +187,15 @@ AP_GPS_None     g_gps_driver(NULL);
   AP_InertialSensor_Oilpan ins( &adc );
 #endif // CONFIG_IMU_TYPE
 AP_IMU_INS imu( &ins );
-AP_DCM  dcm(&imu, g_gps);
+
+#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);
+#else
+ AP_DCM  dcm(&imu, g_gps);
+#endif
 
 #elif HIL_MODE == HIL_MODE_SENSORS
 // sensor emulators
@@ -709,7 +718,7 @@ static void fast_loop()
 		gcs_update();
 	#endif
 
-	dcm.update_DCM();
+    dcm.update_DCM();
 
 	// uses the yaw from the DCM to give more accurate turns
 	calc_bearing_error();
@@ -771,8 +780,13 @@ static void medium_loop()
 			#if HIL_MODE != HIL_MODE_ATTITUDE
             if (g.compass_enabled && compass.read()) {
                 dcm.set_compass(&compass);
-                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
             } else {
                 dcm.set_compass(NULL);
             }

ArduPlane/GCS_Mavlink.pde

@@ -306,6 +306,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,
@@ -318,6 +319,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
         g_gps->ground_speed * rot.b.x,  // Y speed cm/s
         g_gps->ground_speed * rot.c.x,
         g_gps->ground_course);          // course in 1/100 degree
+#endif
 }
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)

ArduPlane/Makefile

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

ArduPlane/config.h

@@ -833,3 +833,8 @@
 #ifndef RESET_SWITCH_CHAN_PWM
 # define RESET_SWITCH_CHAN_PWM 1750
 #endif
+
+// experimental quaternion code
+#ifndef QUATERNION_ENABLE
+# define QUATERNION_ENABLE DISABLED
+#endif

ArduPlane/test.pde

@@ -534,7 +534,12 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
 					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());
+#endif
                     }
                     medium_loopCounter = 0;
 				}
@@ -599,8 +604,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
             medium_loopCounter++;
             if(medium_loopCounter == 5){
                 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
                 }
                 medium_loopCounter = 0;
             }