bool AP_Compass_HMC5843::init()
{
hal.scheduler->suspend_timer_procs();
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("HMC5843: Unable to get bus semaphore\n");
goto fail_sem;
}
if (!_bus->configure()) {
hal.console->printf("HMC5843: Could not configure the bus\n");
goto errout;
}
if (!_detect_version()) {
hal.console->printf("HMC5843: Could not detect version\n");
goto errout;
}
if (!_calibrate()) {
hal.console->printf("HMC5843: Could not calibrate sensor\n");
goto errout;
}
if (!_setup_sampling_mode()) {
goto errout;
}
if (!_bus->start_measurements()) {
hal.console->printf("HMC5843: Could not start measurements on bus\n");
goto errout;
}
_initialised = true;
bus_sem->give();
hal.scheduler->resume_timer_procs();
// perform an initial read
read();
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
if (_force_external) {
set_external(_compass_instance, true);
}
return true;
errout:
bus_sem->give();
fail_sem:
hal.scheduler->resume_timer_procs();
return false;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
_bus_sem = _bus->get_semaphore();
hal.scheduler->suspend_timer_procs();
if (!_bus_sem || !_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf_P(PSTR("HMC5843: Unable to get bus semaphore\n"));
goto fail_sem;
}
if (!_bus->configure()) {
hal.console->printf_P(PSTR("HMC5843: Could not configure the bus\n"));
goto errout;
}
if (!_detect_version()) {
hal.console->printf_P(PSTR("HMC5843: Could not detect version\n"));
goto errout;
}
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
calibration_gain = 0x60;
/*
note that the HMC5883 datasheet gives the x and y expected
values as 766 and the z as 713. Experiments have shown the x
axis is around 766, and the y and z closer to 713.
*/
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0f / 1090; // adjustment for runtime vs calibration gain
}
if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
hal.console->printf_P(PSTR("HMC5843: Could not calibrate sensor\n"));
goto errout;
}
// leave test mode
if (!re_initialise()) {
goto errout;
}
if (!_bus->start_measurements()) {
hal.console->printf_P(PSTR("HMC5843: Could not start measurements on bus\n"));
goto errout;
}
_initialised = true;
_bus_sem->give();
hal.scheduler->resume_timer_procs();
// perform an initial read
read();
#if 0
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"),
_scaling[0], _scaling[1], _scaling[2]);
#endif
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
return true;
errout:
_bus_sem->give();
fail_sem:
hal.scheduler->resume_timer_procs();
return false;
}