From 0248b48d308ca67434df0c8b4960fd86bca8fd68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 Feb 2012 14:26:54 +1100 Subject: [PATCH] allow MAG_ENABLE to be changed in flight this disables the compass in DCM if MAG_ENABLE is changed in flight. Without this we would use a fixed yaw once the compass is disabled This also makes sure we don't pass the compass to DCM till we have done a read. This ensures we have a good compass fix for the initial DCM heading --- ArduCopter/sensors.pde | 2 +- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/system.pde | 3 +-- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index c00c0c54fd..4c901b6357 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -78,7 +78,7 @@ static int32_t read_barometer(void) static void init_compass() { compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { + if (!compass.init() || !compass.read()) { // make sure we don't pass a broken compass to DCM Serial.println_P(PSTR("COMPASS INIT ERROR")); return; diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 41b092d130..edc7963f3b 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -769,8 +769,11 @@ 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 compass.null_offsets(dcm.get_dcm_matrix()); + } else { + dcm.set_compass(NULL); } #endif /*{ diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 2224508ee5..39017f4248 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -175,12 +175,11 @@ static void init_ardupilot() if (g.compass_enabled==true) { compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { + if (!compass.init() || !compass.read()) { Serial.println_P(PSTR("Compass initialisation failed!")); g.compass_enabled = false; } else { dcm.set_compass(&compass); - compass.get_offsets(); // load offsets to account for airframe magnetic interference compass.null_offsets_enable(); } }