From 371677610de162e295423887e5bb7095f77fb147 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 7 Mar 2012 15:13:13 +1100
Subject: [PATCH] APM: avoid fetching the DCM matrix twice

also no special case for quaternions
---
 ArduPlane/ArduPlane.pde | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index b7ff74cb80..d343e9ee6b 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -781,12 +781,9 @@ static void medium_loop()
             if (g.compass_enabled && compass.read()) {
                 dcm.set_compass(&compass);
                 // 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
+                Matrix3f m = dcm.get_dcm_matrix();
+                compass.calculate(m);
+                compass.null_offsets(m);
             } else {
                 dcm.set_compass(NULL);
             }