Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_HAL/AP_HAL_Boards.h>
       4             : 
       5             : #if AP_SIM_ENABLED
       6             : 
       7             : #include <AP_Math/AP_Math.h>
       8             : #include <AP_Math/vectorN.h>
       9             : #include <AP_Common/AP_Common.h>
      10             : #include <AP_Compass/AP_Compass_config.h>
      11             : 
      12             : namespace SITL {
      13             : 
      14             : /*
      15             :   simulation of what one compass sensor reports.
      16             : 
      17             :   This lives here rather than in AP_Compass_SITL because several
      18             :   simulated devices want the same numbers: the directly-attached
      19             :   compasses, the DroneCAN compass, and (for the offsets a
      20             :   perfectly-calibrated compass would end up with) AP_AHRS_SIM.
      21             : 
      22             :   Each consumer owns its own instance of this object.  Sampling is
      23             :   deliberately left per-consumer: sharing one set of samples would mean
      24             :   whichever device asked first consumed them, and the sensors are only
      25             :   simulated at 100Hz, so nothing is gained by sharing the work.
      26             :  */
      27             : class CompassSim {
      28             : public:
      29       16744 :     CompassSim(uint8_t _instance) : instance{_instance} {}
      30             : 
      31             :     /*
      32             :       fill "field" with the field this compass would report, in
      33             :       milligauss.  Returns false if no new sample is due -- the sensor
      34             :       is simulated at 100Hz -- or if the sensor is failed
      35             :       (SIM_MAGn_FAIL=1); the caller should not accumulate a sample in
      36             :       either case.
      37             :      */
      38             :     bool update(Vector3f &field);
      39             : 
      40             : private:
      41             :     const uint8_t instance;
      42             : 
      43             :     // create the correction matrix for diagonals and off-diagonals
      44             :     void setup_eliptical_correction();
      45             : 
      46             :     // delay buffer, so SIM_MAG_DELAY can be simulated
      47             :     struct readings_compass {
      48             :         uint32_t time;
      49             :         Vector3f data;
      50             :     };
      51             :     static const uint8_t buffer_length = 50;
      52             :     VectorN<readings_compass, buffer_length> buffer;
      53             :     uint8_t store_index;
      54             :     uint32_t last_store_time;
      55             : 
      56             :     uint32_t last_sample_time;
      57             : 
      58             :     Matrix3f eliptical_corr;
      59             :     Vector3f last_dia;
      60             :     Vector3f last_odi;
      61             : 
      62             :     // last field reported, so SIM_MAGn_FAIL=2 can freeze it
      63             :     Vector3f last_data;
      64             : };
      65             : 
      66             : }  // namespace SITL
      67             : 
      68             : #endif  // AP_SIM_ENABLED