From e2c525434ad37ef8c9f8cd0b1891ab520f783558 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 7 Mar 2012 15:12:50 +1100 Subject: [PATCH] ACM: removed quaternion special cases in CLI code --- ArduCopter/test.pde | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 00a3dcb89d..f828dabf3c 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -329,13 +329,10 @@ test_radio(uint8_t argc, const Menu::arg *argv) if(g.compass_enabled){ medium_loopCounter++; if(medium_loopCounter == 5){ + Matrix3f m = dcm.get_dcm_matrix(); 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 + compass.calculate(m); + compass.null_offsets(m); medium_loopCounter = 0; } } @@ -552,12 +549,9 @@ 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 + Matrix3f m = dcm.get_dcm_matrix(); + compass.calculate(m); + compass.null_offsets(m); } } fast_loopTimer = millis(); @@ -941,11 +935,7 @@ 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,