AP_Compass: move SITL Compass to standard sensor backend model

libraries/AP_Compass/AP_Compass.cpp

@@ -5,6 +5,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
+#include "AP_Compass_SITL.h"
 #include "AP_Compass_AK8963.h"
 #include "AP_Compass_Backend.h"
 #include "AP_Compass_BMM150.h"
@@ -513,7 +514,12 @@ void Compass::_detect_backends(void)
           return; \
         } \
     } while (0)
-    
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    ADD_BACKEND(new AP_Compass_SITL(*this), nullptr, false);
+    return;
+#endif
+
 #if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
     ADD_BACKEND(AP_Compass_HIL::detect(*this), nullptr, false);
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN

libraries/AP_Compass/AP_Compass.h

@@ -261,6 +261,9 @@ public:
     uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; }
     uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; }
 
+    uint32_t last_update_ms(void) const { return _state[get_primary()].last_update_ms; }
+    uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
     // HIL variables
@@ -286,7 +289,7 @@ public:
     uint16_t get_offsets_max(void) const {
         return (uint16_t)_offset_max.get();
     }
-    
+
 private:
     /// Register a new compas driver, allocating an instance number
     ///
@@ -388,7 +391,7 @@ private:
     } _state[COMPASS_MAX_INSTANCES];
 
     AP_Int16 _offset_max;
-    
+
     CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
 
     // if we want HIL only

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -0,0 +1,113 @@
+#include "AP_Compass_SITL.h"
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+extern const AP_HAL::HAL& hal;
+
+AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
+    _has_sample(false),
+    AP_Compass_Backend(compass)
+{
+    _sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
+    if (_sitl != nullptr) {
+        _compass._setup_earth_field();
+        for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+            _compass_instance[i] = register_compass();
+        }
+        hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
+    }
+}
+
+void AP_Compass_SITL::_timer()
+{
+    // TODO: Refactor delay buffer with AP_Baro_SITL.
+
+    // Sampled at 100Hz
+    uint32_t now = AP_HAL::millis();
+    if ((now - _last_sample_time) < 10) {
+        return;
+    }
+    _last_sample_time = now;
+
+    // calculate sensor noise and add to 'truth' field in body frame
+    // units are milli-Gauss
+    Vector3f noise = rand_vec3f() * _sitl->mag_noise;
+    Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
+
+    // add delay
+    uint32_t best_time_delta = 1000; // initialise large time representing buffer entry closest to current time - delay.
+    uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay.
+
+    // storing data from sensor to buffer
+    if (now - last_store_time >= 10) { // store data every 10 ms.
+        last_store_time = now;
+        if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer
+            store_index = 0;
+        }
+        buffer[store_index].data = new_mag_data; // add data to current index
+        buffer[store_index].time = last_store_time; // add time to current index
+        store_index = store_index + 1; // increment index
+    }
+
+    // return delayed measurement
+    uint32_t delayed_time = now - _sitl->mag_delay; // get time corresponding to delay
+    // find data corresponding to delayed time in buffer
+    for (uint8_t i=0; i<=buffer_length-1; i++) {
+        // find difference between delayed time and time stamp in buffer
+        uint32_t time_delta = abs((int32_t)(delayed_time - buffer[i].time));
+        // if this difference is smaller than last delta, store this time
+        if (time_delta < best_time_delta) {
+            best_index= i;
+            best_time_delta = time_delta;
+        }
+    }
+    if (best_time_delta < 1000) { // only output stored state if < 1 sec retrieval error
+        new_mag_data = buffer[best_index].data;
+    }
+
+    new_mag_data -= _sitl->mag_ofs.get();
+
+    for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+        rotate_field(new_mag_data, _compass_instance[i]);
+        publish_raw_field(new_mag_data, AP_HAL::micros(), _compass_instance[i]);
+        correct_field(new_mag_data, _compass_instance[i]);
+    }
+
+    if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        return;
+    }
+
+    _mag_accum += new_mag_data;
+    _accum_count++;
+    if (_accum_count == 10) {
+        _mag_accum /= 2;
+        _accum_count = 5;
+        _has_sample = true;
+    }
+    _sem->give();
+}
+
+void AP_Compass_SITL::read()
+{
+    if (_sem->take_nonblocking()) {
+        if (!_has_sample) {
+            _sem->give();
+            return;
+        }
+
+        Vector3f field(_mag_accum);
+        field /= _accum_count;
+        _mag_accum.zero();
+        _accum_count = 0;
+
+        for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+            publish_filtered_field(field, _compass_instance[i]);
+        }
+
+        _has_sample = false;
+        _sem->give();
+    }
+
+}
+#endif

libraries/AP_Compass/AP_Compass_SITL.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <SITL/SITL.h>
+#include <AP_Math/vectorN.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Declination/AP_Declination.h>
+
+#define SITL_NUM_COMPASSES 2
+
+class AP_Compass_SITL : public AP_Compass_Backend {
+public:
+    AP_Compass_SITL(Compass &);
+
+    void read(void);
+
+private:
+    uint8_t _compass_instance[SITL_NUM_COMPASSES];
+    SITL::SITL *_sitl;
+
+    // delay buffer variables
+    struct readings_compass {
+        uint32_t time;
+        Vector3f data;
+    };
+    uint8_t store_index;
+    uint32_t last_store_time;
+    static const uint8_t buffer_length = 50;
+    VectorN<readings_compass,buffer_length> buffer;
+
+    void _timer();
+    bool _has_sample;
+    uint32_t _last_sample_time;
+
+    Vector3f _mag_accum;
+    uint32_t _accum_count;
+
+
+};
+#endif // CONFIG_HAL_BOARD