AP_Compass: move the compass field simulation into the SITL library

AP_Compass_SITL held the compass sensor model -- the noise, the delay
buffer, the elliptical correction and the failure injection -- inline.
Move it to SITL::CompassSim so it sits alongside the rest of the sensor
simulation and beside the transformation it shares with
get_mag_offsets().

The consumer owns its own instance, so sampling stays where it was and
the 100Hz rate at which the sensor is simulated is unchanged.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -47,96 +47,14 @@ AP_Compass_SITL::AP_Compass_SITL(uint8_t _sitl_instance) :
 }
 
 
-/*
-  create correction matrix for diagonals and off-diagonals
-*/
-void AP_Compass_SITL::_setup_eliptical_correcion()
-{
-    Vector3f diag = _sitl->mag_diag[sitl_instance].get();
-    if (diag.is_zero()) {
-        diag = {1,1,1};
-    }
-    const Vector3f &diagonals = diag;
-    const Vector3f &offdiagonals = _sitl->mag_offdiag[sitl_instance];
-    
-    if (diagonals == _last_dia && offdiagonals == _last_odi) {
-        return;
-    }
-    
-    _eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
-                               offdiagonals.x, diagonals.y,    offdiagonals.z,
-                               offdiagonals.y, offdiagonals.z, diagonals.z);
-    if (!_eliptical_corr.invert()) {
-        _eliptical_corr.identity();
-    }
-    _last_dia = diag;
-    _last_odi = offdiagonals;
-}
-
 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) {
+    Vector3f f;
+    if (!_compass_sim.update(f)) {
         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;
-    }
-
-        _setup_eliptical_correcion();
-        Vector3f f = (_eliptical_corr * new_mag_data) - _sitl->mag_ofs[sitl_instance].get();
-        _sitl->mag_sensor_transform(sitl_instance, f);
-
-        switch (_sitl->mag_fail[sitl_instance]) {
-        case 0:
-            accumulate_sample(f, 10);
-            _last_data = f;
-            break;
-        case 1:
-            // no data
-            break;
-        case 2:
-            // frozen compass
-            accumulate_sample(_last_data, 10);
-            break;
-        }
+    accumulate_sample(f, 10);
 }
 
 #endif  // AP_COMPASS_SITL_ENABLED

libraries/AP_Compass/AP_Compass_SITL.h

@@ -6,10 +6,10 @@
 
 #include "AP_Compass_Backend.h"
 
-#include <AP_Math/vectorN.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_Declination/AP_Declination.h>
 #include <SITL/SITL.h>
+#include <SITL/SIM_Compass.h>
 
 class AP_Compass_SITL : public AP_Compass_Backend {
 public:
@@ -18,25 +18,11 @@ public:
 private:
     SITL::SIM *_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();
-    uint32_t _last_sample_time;
-
-    void _setup_eliptical_correcion();
 
     uint8_t sitl_instance;  // offset into SITL state structure arrays
-    Matrix3f _eliptical_corr;
-    Vector3f _last_dia;
-    Vector3f _last_odi;
-    Vector3f _last_data;
+
+    // the simulation of what this compass reports:
+    SITL::CompassSim _compass_sim{sitl_instance};
 };
 #endif // AP_COMPASS_SITL_ENABLED