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;
}
bool AP_Compass_AK8963::init()
{
_healthy[0] = true;
_field[0].x = 0.0f;
_field[0].y = 0.0f;
_field[0].z = 0.0f;
hal.scheduler->suspend_timer_procs();
if (!_backend->sem_take_blocking()) {
error("_spi_sem->take failed\n");
return false;
}
if (!_backend_init()) {
_backend->sem_give();
return false;
}
_register_write(AK8963_CNTL2, AK8963_RESET); /* Reset AK8963 */
hal.scheduler->delay(1000);
int id_mismatch_count;
uint8_t deviceid;
for (id_mismatch_count = 0; id_mismatch_count < 5; id_mismatch_count++) {
_register_read(AK8963_WIA, 0x01, &deviceid); /* Read AK8963's id */
if (deviceid == AK8963_Device_ID) {
break;
}
error("trying to read AK8963's ID once more...\n");
_backend_reset();
hal.scheduler->delay(100);
_dump_registers();
}
if (id_mismatch_count == 5) {
_initialised = false;
hal.console->printf("WRONG AK8963 DEVICE ID: 0x%x\n", (unsigned)deviceid);
hal.scheduler->panic(PSTR("AK8963: bad DEVICE ID"));
}
_calibrate();
_initialised = true;
#if AK8963_SELFTEST
if (_self_test()) {
_initialised = true;
} else {
_initialised = false;
}
#endif
/* Register value to continuous measurement */
_register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution);
_backend->sem_give();
hal.scheduler->resume_timer_procs();
hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Compass_AK8963::_update));
_start_conversion();
_initialised = true;
return _initialised;
}
// 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;
}