Line data    Source code

       1             : #include "AP_Compass_SITL.h"
       2             : 
       3             : #if AP_COMPASS_SITL_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9       16744 : AP_Compass_SITL::AP_Compass_SITL(uint8_t _sitl_instance) :
      10       33488 :     _sitl(AP::sitl()),
      11       16744 :     sitl_instance(_sitl_instance)
      12             : {
      13       16744 :     if (sitl_instance > ARRAY_SIZE(_sitl->mag_devid)) {
      14           0 :         return;
      15             :     }
      16             : 
      17       16744 :             const uint32_t dev_id = _sitl->mag_devid[sitl_instance];
      18       16744 :             if (dev_id == 0) {
      19        4186 :                 return;
      20             :             }
      21       12558 :             if (!register_compass(dev_id)) {
      22        6279 :                 return;
      23             :             }
      24        6279 :                 if (_sitl->mag_save_ids) {
      25             :                     // save so the compass always comes up configured in SITL
      26        6264 :                     save_dev_id();
      27             :                 }
      28        6279 :                 set_rotation(ROTATION_NONE);
      29             : 
      30             :         // Scroll through the registered compasses, and set the offsets
      31        6279 :             if (_compass.get_offsets(instance).is_zero()) {
      32        2388 :                 _compass.set_offsets(instance, _sitl->mag_ofs[sitl_instance]);
      33             :             }
      34             : 
      35             :         // we want to simulate a calibrated compass by default, so set
      36             :         // scale to 1
      37        6279 :         AP_Param::set_default_by_name("COMPASS_SCALE", 1);
      38        6279 :         AP_Param::set_default_by_name("COMPASS_SCALE2", 1);
      39        6279 :         AP_Param::set_default_by_name("COMPASS_SCALE3", 1);
      40             : 
      41             :         // make first compass external
      42        6279 :         if (sitl_instance == 0) {
      43        2092 :             set_external(true);
      44             :         }
      45             : 
      46        6279 :         hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
      47             : }
      48             : 
      49             : 
      50   560287338 : void AP_Compass_SITL::_timer()
      51             : {
      52   560287338 :     Vector3f f;
      53   560287338 :     if (!_compass_sim.update(f)) {
      54   513636929 :         return;
      55             :     }
      56             : 
      57    46650409 :     accumulate_sample(f, 10);
      58             : }
      59             : 
      60             : #endif  // AP_COMPASS_SITL_ENABLED