From 36190ba545c2a93b881981162b3663d3fb9a7ed3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 3 Nov 2016 21:03:45 +1100
Subject: [PATCH] AP_Compass: use thread per bus for lsm303d mag

---
 libraries/AP_Compass/AP_Compass_LSM303D.cpp | 73 +++++++++------------
 libraries/AP_Compass/AP_Compass_LSM303D.h   |  4 +-
 2 files changed, 34 insertions(+), 43 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp
index a9f9b344ff..21e9819d10 100644
--- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp
+++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp
@@ -208,7 +208,7 @@ void AP_Compass_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_
  */
 bool AP_Compass_LSM303D::_data_ready()
 {
-    return (_drdy_pin_m->read()) != 0;
+    return _drdy_pin_m == nullptr || (_drdy_pin_m->read() != 0);
 }
 
 
@@ -253,17 +253,12 @@ bool AP_Compass_LSM303D::_read_sample()
 
 bool AP_Compass_LSM303D::init()
 {
-    // TODO: support users without data ready pin
-    if (LSM303D_DRDY_M_PIN < 0) {
-        return false;
+    if (LSM303D_DRDY_M_PIN >= 0) {
+        _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN);
+        _drdy_pin_m->mode(HAL_GPIO_INPUT);
     }
 
-    _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN);
-    _drdy_pin_m->mode(HAL_GPIO_INPUT);
-
-    hal.scheduler->suspend_timer_procs();
     bool success = _hardware_init();
-    hal.scheduler->resume_timer_procs();
 
     if (!success) {
         return false;
@@ -280,7 +275,10 @@ bool AP_Compass_LSM303D::init()
     set_external(_compass_instance, false);
 #endif
 
-    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
+    _accum_sem = hal.util->new_semaphore();
+    
+    // read at 100Hz
+    _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, bool));
 
     return true;
 }
@@ -339,49 +337,37 @@ fail_whoami:
     return false;
 }
 
-void AP_Compass_LSM303D::_update()
+bool AP_Compass_LSM303D::_update()
 {
-    uint32_t time_us = AP_HAL::micros();
-    Vector3f raw_field;
-
-    if (AP_HAL::micros() - _last_update_timestamp < 10000) {
-        return;
-    }
-
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return;
-    }
-
     if (!_read_sample()) {
-        goto fail;
+        return true;
     }
 
-    raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
+    Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
 
     // rotate raw_field from sensor frame to body frame
     rotate_field(raw_field, _compass_instance);
 
     // publish raw_field (uncorrected point sample) for calibration use
-    publish_raw_field(raw_field, time_us, _compass_instance);
+    publish_raw_field(raw_field, AP_HAL::micros(), _compass_instance);
 
     // correct raw_field for known errors
     correct_field(raw_field, _compass_instance);
 
-    _mag_x_accum += raw_field.x;
-    _mag_y_accum += raw_field.y;
-    _mag_z_accum += raw_field.z;
-    _accum_count++;
-    if (_accum_count == 10) {
-        _mag_x_accum /= 2;
-        _mag_y_accum /= 2;
-        _mag_z_accum /= 2;
-        _accum_count = 5;
+    if (_accum_sem->take(0)) {
+        _mag_x_accum += raw_field.x;
+        _mag_y_accum += raw_field.y;
+        _mag_z_accum += raw_field.z;
+        _accum_count++;
+        if (_accum_count == 10) {
+            _mag_x_accum /= 2;
+            _mag_y_accum /= 2;
+            _mag_z_accum /= 2;
+            _accum_count = 5;
+        }
+        _accum_sem->give();
     }
-
-    _last_update_timestamp = AP_HAL::micros();
-
-fail:
-    _dev->get_semaphore()->give();
+    return true;
 }
 
 // Read Sensor data
@@ -391,18 +377,23 @@ void AP_Compass_LSM303D::read()
         return;
     }
 
+    if (!_accum_sem->take_nonblocking()) {
+        return;
+    }
+    
     if (_accum_count == 0) {
         /* We're not ready to publish*/
+        _accum_sem->give();
         return;
     }
 
-    hal.scheduler->suspend_timer_procs();
     Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
     field /= _accum_count;
 
     _accum_count = 0;
     _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
-    hal.scheduler->resume_timer_procs();
+
+    _accum_sem->give();    
 
     publish_filtered_field(field, _compass_instance);
 }
diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.h b/libraries/AP_Compass/AP_Compass_LSM303D.h
index 3a4f986972..c55bc8e795 100644
--- a/libraries/AP_Compass/AP_Compass_LSM303D.h
+++ b/libraries/AP_Compass/AP_Compass_LSM303D.h
@@ -33,7 +33,7 @@ private:
 
     bool _data_ready();
     bool _hardware_init();
-    void _update();
+    bool _update();
     void _disable_i2c();
     bool _mag_set_range(uint8_t max_ga);
     bool _mag_set_samplerate(uint16_t frequency);
@@ -45,7 +45,6 @@ private:
     float _mag_x_accum;
     float _mag_y_accum;
     float _mag_z_accum;
-    uint32_t _last_update_timestamp;
     int16_t _mag_x;
     int16_t _mag_y;
     int16_t _mag_z;
@@ -57,4 +56,5 @@ private:
     uint8_t _mag_range_ga;
     uint8_t _mag_samplerate;
     uint8_t _reg7_expected;
+    AP_HAL::Semaphore *_accum_sem;
 };