Browse Source

AP_Compass: HMC5843: remove useless delay

There's no need to add a delay after suspending timer in the
initialization. Also initialize _bus_sem earlier, like is done in
AK8963.
master
Lucas De Marchi 10 years ago committed by Andrew Tridgell
parent
commit
9ecd1daf81
  1. 3
      libraries/AP_Compass/AP_Compass_HMC5843.cpp

3
libraries/AP_Compass/AP_Compass_HMC5843.cpp

@ -244,10 +244,9 @@ AP_Compass_HMC5843::init() @@ -244,10 +244,9 @@ AP_Compass_HMC5843::init()
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
_bus_sem = _bus->get_semaphore();
hal.scheduler->suspend_timer_procs();
hal.scheduler->delay(10);
_bus_sem = _bus->get_semaphore();
if (!_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore"));
}

Loading…
Cancel
Save