Line data    Source code

       1             : #include "AP_Compass_config.h"
       2             : 
       3             : #if AP_COMPASS_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : 
       7             : #include "AP_Compass.h"
       8             : #include "AP_Compass_Backend.h"
       9             : 
      10             : #include <AP_BattMonitor/AP_BattMonitor.h>
      11             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      12             : #include <AP_BoardConfig/AP_BoardConfig.h>
      13             : 
      14       16806 : AP_Compass_Backend::AP_Compass_Backend()
      15       16806 :     : _compass(AP::compass())
      16             : {
      17       16806 : }
      18             : 
      19    46650409 : void AP_Compass_Backend::rotate_field(Vector3f &mag)
      20             : {
      21    46650409 :     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
      22             :     if (MAG_BOARD_ORIENTATION != ROTATION_NONE) {
      23             :         mag.rotate(MAG_BOARD_ORIENTATION);
      24             :     }
      25    46650409 :     mag.rotate(state.rotation);
      26             : 
      27             : #ifdef HAL_HEATER_MAG_OFFSET
      28             :     /*
      29             :       apply compass compensations for boards that have a heater which
      30             :       interferes with an internal compass. This needs to be applied
      31             :       before the board orientation so it is independent of
      32             :       AHRS_ORIENTATION
      33             :      */
      34             :     if (!is_external()) {
      35             :         const uint32_t dev_id = uint32_t(_compass._state[Compass::StateIndex(instance)].dev_id);
      36             :         static const struct offset {
      37             :             uint32_t dev_id;
      38             :             Vector3f ofs;
      39             :         } offsets[] = HAL_HEATER_MAG_OFFSET;
      40             :         const auto *bc = AP::boardConfig();
      41             :         if (bc) {
      42             :             for (const auto &o : offsets) {
      43             :                 if (o.dev_id == dev_id) {
      44             :                     mag += o.ofs * bc->get_heater_duty_cycle() * 0.01;
      45             :                 }
      46             :             }
      47             :         }
      48             :     }
      49             : #endif
      50             : 
      51    46650409 :     if (!state.external) {
      52    31082791 :         mag.rotate(_compass._board_orientation);
      53             :     } else {
      54             :         // add user selectable orientation
      55    15567618 :         mag.rotate((enum Rotation)state.orientation.get());
      56             :     }
      57    46650409 : }
      58             : 
      59    46650409 : void AP_Compass_Backend::publish_raw_field(const Vector3f &mag)
      60             : {
      61             :     // note that we do not set last_update_usec here as otherwise the
      62             :     // EKF and DCM would end up consuming compass data at the full
      63             :     // sensor rate. We want them to consume only the filtered fields
      64             : #if COMPASS_CAL_ENABLED
      65    46650409 :     auto& cal = _compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))];
      66    46650409 :     if (cal != nullptr) {
      67      596089 :         Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
      68      596089 :         state.last_update_ms = AP_HAL::millis();
      69      596089 :         cal->new_sample(mag);
      70             :     }
      71             : #endif
      72    46650409 : }
      73             : 
      74    46650409 : void AP_Compass_Backend::correct_field(Vector3f &mag)
      75             : {
      76    46650409 :     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
      77             : 
      78    46650409 :     const Vector3f &offsets = state.offset.get();
      79             : #if AP_COMPASS_DIAGONALS_ENABLED
      80    46650409 :     const Vector3f &diagonals = state.diagonals.get();
      81    46650409 :     const Vector3f &offdiagonals = state.offdiagonals.get();
      82             : #endif
      83             : 
      84             :     // add in the basic offsets
      85    46650409 :     mag += offsets;
      86             : 
      87             :     // add in scale factor, use a wide sanity check. The calibrator
      88             :     // uses a narrower check.
      89    46650409 :     if (_compass.have_scale_factor(instance)) {
      90    46566542 :         mag *= state.scale_factor;
      91             :     }
      92             : 
      93             : #if AP_COMPASS_DIAGONALS_ENABLED
      94             :     // apply elliptical correction
      95    46650409 :     if (!diagonals.is_zero()) {
      96             :         Matrix3f mat(
      97    46650409 :             diagonals.x,    offdiagonals.x, offdiagonals.y,
      98    46650409 :             offdiagonals.x, diagonals.y,    offdiagonals.z,
      99    46650409 :             offdiagonals.y, offdiagonals.z, diagonals.z
     100    46650409 :             );
     101             : 
     102    46650409 :         mag = mat * mag;
     103             :     }
     104             : #endif
     105             : 
     106             : #if COMPASS_MOT_ENABLED
     107    46650409 :     const Vector3f &mot = state.motor_compensation.get();
     108             :     /*
     109             :       calculate motor-power based compensation
     110             :       note that _motor_offset[] is kept even if compensation is not
     111             :       being applied so it can be logged correctly
     112             :     */    
     113    46650409 :     state.motor_offset.zero();
     114    46650409 :     if (_compass._per_motor.enabled() && instance == 0) {
     115             :         // per-motor correction is only valid for first compass
     116           0 :         _compass._per_motor.compensate(state.motor_offset);
     117    46650409 :     } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
     118           0 :         state.motor_offset = mot * _compass._thr;
     119    46650409 :     } else if (_compass._motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
     120        1938 :         AP_BattMonitor &battery = AP::battery();
     121             :         float current;
     122        1938 :         if (battery.current_amps(current)) {
     123        1938 :             state.motor_offset = mot * current;
     124             :         }
     125             :     }
     126             : 
     127             :     /*
     128             :       we apply the motor offsets after we apply the elliptical
     129             :       correction. This is needed to match the way that the motor
     130             :       compensation values are calculated, as they are calculated based
     131             :       on final field outputs, not on the raw outputs
     132             :     */
     133    46650409 :     mag += state.motor_offset;
     134             : #endif // COMPASS_MOT_ENABLED
     135    46650409 : }
     136             : 
     137    46650409 : void AP_Compass_Backend::accumulate_sample(Vector3f &field,
     138             :                                            uint32_t max_samples)
     139             : {
     140             :     /* rotate raw_field from sensor frame to body frame */
     141    46650409 :     rotate_field(field);
     142             : 
     143             :     /* publish raw_field (uncorrected point sample) for calibration use */
     144    46650409 :     publish_raw_field(field);
     145             : 
     146             :     /* correct raw_field for known errors */
     147    46650409 :     correct_field(field);
     148             : 
     149    46650409 :     if (!field_ok(field)) {
     150           0 :         return;
     151             :     }
     152             : 
     153    46650409 :     WITH_SEMAPHORE(_sem);
     154             : 
     155    46650409 :     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
     156    46650409 :     state.accum += field;
     157    46650409 :     state.accum_count++;
     158    46650409 :     if (max_samples && state.accum_count >= max_samples) {
     159     4801253 :         state.accum_count /= 2;
     160     4801253 :         state.accum /= 2;
     161             :     }
     162    46650409 : }
     163             : 
     164     4524327 : void AP_Compass_Backend::drain_accumulated_samples(const Vector3f *scaling)
     165             : {
     166     4524327 :     WITH_SEMAPHORE(_sem);
     167             : 
     168     4524327 :     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
     169             : 
     170     4524327 :     if (state.accum_count == 0) {
     171        5825 :         return;
     172             :     }
     173             : 
     174     4518502 :     if (scaling) {
     175           0 :         state.accum *= *scaling;
     176             :     }
     177     4518502 :     state.accum /= state.accum_count;
     178             : 
     179     4518502 :     publish_filtered_field(state.accum);
     180             : 
     181     4518502 :     state.accum.zero();
     182     4518502 :     state.accum_count = 0;
     183     4524327 : }
     184             : 
     185             : /*
     186             :   copy latest data to the frontend from a backend
     187             :  */
     188     4518502 : void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag)
     189             : {
     190     4518502 :     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
     191             : 
     192     4518502 :     state.field = mag;
     193             : 
     194     4518502 :     state.last_update_ms = AP_HAL::millis();
     195     4518502 :     state.last_update_usec = AP_HAL::micros();
     196     4518502 : }
     197             : 
     198           0 : void AP_Compass_Backend::set_last_update_usec(uint32_t last_update)
     199             : {
     200           0 :     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
     201           0 :     state.last_update_usec = last_update;
     202           0 : }
     203             : 
     204             : /*
     205             :   register a new backend with frontend, returning instance which
     206             :   should be used in publish_field()
     207             :  */
     208       12620 : bool AP_Compass_Backend::register_compass(int32_t dev_id)
     209             : {
     210       12620 :     if (!_compass.register_compass(dev_id, instance)) {
     211        6341 :         return false;
     212             :     }
     213        6279 :     set_dev_id(dev_id);
     214        6279 :     return true;
     215             : }
     216             : 
     217             : /*
     218             :   set dev_id for an instance
     219             : */
     220        6279 : void AP_Compass_Backend::set_dev_id(uint32_t dev_id)
     221             : {
     222        6279 :     _compass._state[Compass::StateIndex(instance)].dev_id.set_and_notify(dev_id);
     223        6279 :     _compass._state[Compass::StateIndex(instance)].detected_dev_id = dev_id;
     224        6279 : }
     225             : 
     226             : /*
     227             :   save dev_id, used by SITL
     228             : */
     229        6264 : void AP_Compass_Backend::save_dev_id()
     230             : {
     231        6264 :     _compass._state[Compass::StateIndex(instance)].dev_id.save();
     232        6264 : }
     233             : 
     234             : /*
     235             :   set external for an instance
     236             : */
     237        2092 : void AP_Compass_Backend::set_external(bool external)
     238             : {
     239        2092 :     if (_compass._state[Compass::StateIndex(instance)].external != 2) {
     240        2087 :         _compass._state[Compass::StateIndex(instance)].external.set_and_notify(external);
     241             :     }
     242        2092 : }
     243             : 
     244           0 : bool AP_Compass_Backend::is_external()
     245             : {
     246           0 :     return _compass._state[Compass::StateIndex(instance)].external;
     247             : }
     248             : 
     249             : // set rotation of an instance
     250        6279 : void AP_Compass_Backend::set_rotation(enum Rotation rotation)
     251             : {
     252        6279 :     _compass._state[Compass::StateIndex(instance)].rotation = rotation;
     253        6279 : }
     254             : 
     255             : static constexpr float FILTER_KOEF = 0.1f;
     256             : 
     257             : /* Check that the compass value is valid by using a mean filter. If
     258             :  * the value is further than filter_range from mean value, it is
     259             :  * rejected. 
     260             : */
     261    46650409 : bool AP_Compass_Backend::field_ok(const Vector3f &field)
     262             : {
     263             : 
     264             :     
     265    46650409 :     if (field.is_inf() || field.is_nan()) {
     266           0 :         return false;
     267             :     }
     268             : 
     269    46650409 :     const float range = (float)_compass.get_filter_range();
     270    46650409 :     if (range <= 0) {
     271    46650409 :         return true;
     272             :     }
     273             : 
     274           0 :     const float length = field.length();
     275             : 
     276           0 :     if (is_zero(_mean_field_length)) {
     277           0 :         _mean_field_length = length;
     278           0 :         return true;
     279             :     }
     280             : 
     281           0 :     bool ret = true;
     282           0 :     const float d = fabsf(_mean_field_length - length) / (_mean_field_length + length);  // diff divide by mean value in percent ( with the *200.0f on later line)
     283           0 :     float koeff = FILTER_KOEF;
     284             : 
     285           0 :     if (d * 200.0f > range) {  // check the difference from mean value outside allowed range
     286             :         // printf("\nCompass field length error: mean %f got %f\n", (double)_mean_field_length, (double)length );
     287           0 :         ret = false;
     288           0 :         koeff /= (d * 10.0f);  // 2.5 and more, so one bad sample never change mean more than 4%
     289           0 :         _error_count++;
     290             :     }
     291           0 :     _mean_field_length = _mean_field_length * (1 - koeff) + length * koeff;  // complimentary filter 1/k
     292             : 
     293           0 :     return ret;
     294             : }
     295             : 
     296             : 
     297           0 : enum Rotation AP_Compass_Backend::get_board_orientation(void) const
     298             : {
     299           0 :     return _compass._board_orientation;
     300             : }
     301             : 
     302             : #endif  // AP_COMPASS_ENABLED