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; }