From 7f00dd413f4b233889ff2b2878f2f3c56a55d67c Mon Sep 17 00:00:00 2001
From: Staroselskii Georgii <gosha371@gmail.com>
Date: Thu, 13 Nov 2014 14:30:20 +0300
Subject: [PATCH] AP_Compass: fixed HMC5883's initialisation code

---
 libraries/AP_Compass/AP_Compass_HMC5843.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
index 323849d349..7f0a63cec4 100644
--- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp
+++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
@@ -180,6 +180,7 @@ AP_Compass_HMC5843::init()
     uint16_t expected_yz = 715;
     float gain_multiple = 1.0;
 
+    hal.scheduler->suspend_timer_procs();
     hal.scheduler->delay(10);
 
     _i2c_sem = hal.i2c->get_semaphore();
@@ -193,6 +194,7 @@ AP_Compass_HMC5843::init()
         !read_register(ConfigRegA, &_base_config)) {
         _healthy[0] = false;
         _i2c_sem->give();
+        hal.scheduler->resume_timer_procs();
         return false;
     }
     if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
@@ -212,6 +214,7 @@ AP_Compass_HMC5843::init()
     } else {
         // not behaving like either supported compass type
         _i2c_sem->give();
+        hal.scheduler->resume_timer_procs();
         return false;
     }
 
@@ -298,10 +301,12 @@ AP_Compass_HMC5843::init()
     // leave test mode
     if (!re_initialise()) {
         _i2c_sem->give();
+        hal.scheduler->resume_timer_procs();
         return false;
     }
 
     _i2c_sem->give();
+    hal.scheduler->resume_timer_procs();
     _initialised = true;
 
 	// perform an initial read