Line data    Source code

       1             : #include "SIM_Compass.h"
       2             : 
       3             : #if AP_SIM_ENABLED
       4             : 
       5             : #include "SITL.h"
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <AP_Compass/AP_Compass.h>
       9             : 
      10             : using namespace SITL;
      11             : 
      12             : /*
      13             :   apply the transformation a simulated compass applies to a vector once
      14             :   SIM_MAGn_OFS has been taken off it.  AP_Compass_SITL applies this to
      15             :   the field it reports; get_mag_offsets_for_devid() applies it to the offset
      16             :   itself.  Keeping the two in one place stops them diverging.
      17             :  */
      18    46666723 : void SIM::mag_sensor_transform(uint8_t instance, Vector3f &v) const
      19             : {
      20    46666723 :     v.rotate_inverse((enum Rotation)mag_orient[instance].get());
      21    46666723 :     v.rotate(AP::compass().get_board_orientation());
      22             : 
      23             :     // SIM_BRD_TRIM: rigid board mounting offset (same rotation applied
      24             :     // to the accels and gyros), keeping the compass consistent with the
      25             :     // IMU
      26    46666723 :     const Vector3f &trim = board_trim.get();
      27    46666723 :     if (!trim.is_zero()) {
      28      225774 :         Matrix3f trim_rotation;
      29      225774 :         trim_rotation.from_euler(trim.x, trim.y, trim.z);
      30      225774 :         v = trim_rotation.transposed() * v;
      31             :     }
      32             : 
      33             :     // scale the compass to simulate sensor scale factor errors
      34    46666723 :     v *= mag_scaling[instance];
      35    46666723 : }
      36             : 
      37           3 : bool SIM::get_mag_offsets_for_devid(uint32_t devid, Vector3f &offsets) const
      38             : {
      39           3 :     if (devid == 0) {
      40           0 :         return false;
      41             :     }
      42             : 
      43             :     // AP_Compass_SITL registers each simulated compass with
      44             :     // mag_devid[sitl_instance], so that is what maps a compass back to
      45             :     // the sensor which produced it:
      46           6 :     for (uint8_t instance=0; instance<ARRAY_SIZE(mag_devid); instance++) {
      47           6 :         if (uint32_t(mag_devid[instance].get()) != devid) {
      48           3 :             continue;
      49             :         }
      50           3 :         if (instance >= ARRAY_SIZE(mag_ofs)) {
      51           0 :             return false;
      52             :         }
      53           3 :         offsets = mag_ofs[instance];
      54           3 :         mag_sensor_transform(instance, offsets);
      55           3 :         return true;
      56             :     }
      57             : 
      58           0 :     return false;
      59             : }
      60             : 
      61             : /*
      62             :   create correction matrix for diagonals and off-diagonals
      63             : */
      64    46666720 : void CompassSim::setup_eliptical_correction()
      65             : {
      66    46666720 :     const auto *_sitl = AP::sitl();
      67             : 
      68    46666720 :     Vector3f diag = _sitl->mag_diag[instance].get();
      69    46666720 :     if (diag.is_zero()) {
      70    45839527 :         diag = {1,1,1};
      71             :     }
      72    46666720 :     const Vector3f &diagonals = diag;
      73    46666720 :     const Vector3f &offdiagonals = _sitl->mag_offdiag[instance];
      74             : 
      75    46666720 :     if (diagonals == last_dia && offdiagonals == last_odi) {
      76    46660427 :         return;
      77             :     }
      78             : 
      79       12586 :     eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
      80        6293 :                               offdiagonals.x, diagonals.y,    offdiagonals.z,
      81        6293 :                               offdiagonals.y, offdiagonals.z, diagonals.z);
      82        6293 :     if (!eliptical_corr.invert()) {
      83           2 :         eliptical_corr.identity();
      84             :     }
      85        6293 :     last_dia = diag;
      86        6293 :     last_odi = offdiagonals;
      87             : }
      88             : 
      89   560287338 : bool CompassSim::update(Vector3f &field)
      90             : {
      91   560287338 :     const auto *_sitl = AP::sitl();
      92   560287338 :     if (_sitl == nullptr) {
      93           0 :         return false;
      94             :     }
      95   560287338 :     if (instance >= ARRAY_SIZE(_sitl->mag_ofs)) {
      96      123606 :         return false;
      97             :     }
      98             : 
      99             :     // TODO: Refactor delay buffer with AP_Baro_SITL.
     100             : 
     101             :     // Sampled at 100Hz
     102   560163732 :     const uint32_t now = AP_HAL::millis();
     103   560163732 :     if ((now - last_sample_time) < 10) {
     104   513497012 :         return false;
     105             :     }
     106    46666720 :     last_sample_time = now;
     107             : 
     108             :     // calculate sensor noise and add to 'truth' field in body frame
     109             :     // units are milli-Gauss
     110    46666720 :     const Vector3f noise = rand_vec3f() * _sitl->mag_noise;
     111    46666720 :     Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
     112             : 
     113             :     // add delay
     114    46666720 :     uint32_t best_time_delta = 1000; // initialise large time representing buffer entry closest to current time - delay.
     115    46666720 :     uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay.
     116             : 
     117             :     // storing data from sensor to buffer
     118    46666720 :     if (now - last_store_time >= 10) { // store data every 10 ms.
     119    46666720 :         last_store_time = now;
     120    46666720 :         if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer
     121      930711 :             store_index = 0;
     122             :         }
     123    46666720 :         buffer[store_index].data = new_mag_data; // add data to current index
     124    46666720 :         buffer[store_index].time = last_store_time; // add time to current index
     125    46666720 :         store_index = store_index + 1; // increment index
     126             :     }
     127             : 
     128             :     // return delayed measurement
     129    46666720 :     const uint32_t delayed_time = now - _sitl->mag_delay; // get time corresponding to delay
     130             :     // find data corresponding to delayed time in buffer
     131  2380002720 :     for (uint8_t i=0; i<=buffer_length-1; i++) {
     132             :         // find difference between delayed time and time stamp in buffer
     133  2333336000 :         const uint32_t time_delta = abs((int32_t)(delayed_time - buffer[i].time));
     134             :         // if this difference is smaller than last delta, store this time
     135  2333336000 :         if (time_delta < best_time_delta) {
     136  1188959283 :             best_index = i;
     137  1188959283 :             best_time_delta = time_delta;
     138             :         }
     139             :     }
     140    46666720 :     if (best_time_delta < 1000) { // only output stored state if < 1 sec retrieval error
     141    46666720 :         new_mag_data = buffer[best_index].data;
     142             :     }
     143             : 
     144    46666720 :     setup_eliptical_correction();
     145    46666720 :     Vector3f f = (eliptical_corr * new_mag_data) - _sitl->mag_ofs[instance].get();
     146    46666720 :     _sitl->mag_sensor_transform(instance, f);
     147             : 
     148    46666720 :     switch (_sitl->mag_fail[instance]) {
     149    46650409 :     case 0:
     150    46650409 :         last_data = f;
     151    46650409 :         field = f;
     152    46650409 :         return true;
     153       16311 :     case 1:
     154             :         // no data
     155       16311 :         return false;
     156           0 :     case 2:
     157             :         // frozen compass
     158           0 :         field = last_data;
     159           0 :         return true;
     160             :     }
     161             : 
     162           0 :     return false;
     163             : }
     164             : 
     165             : #endif  // AP_SIM_ENABLED