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_PX4::init(void)
{
_mag_fd[0] = open(MAG_BASE_DEVICE_PATH"0", O_RDONLY);
_mag_fd[1] = open(MAG_BASE_DEVICE_PATH"1", O_RDONLY);
_mag_fd[2] = open(MAG_BASE_DEVICE_PATH"2", O_RDONLY);
_num_sensors = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (_mag_fd[i] >= 0) {
_num_sensors = i+1;
}
}
if (_num_sensors == 0) {
hal.console->printf("Unable to open " MAG_BASE_DEVICE_PATH"0" "\n");
return false;
}
for (uint8_t i=0; i<_num_sensors; i++) {
_instance[i] = register_compass();
// get device id
set_dev_id(_instance[i], ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0));
// average over up to 20 samples
if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup compass queue\n");
return false;
}
// remember if the compass is external
set_external(_instance[i], ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
_count[i] = 0;
_sum[i].zero();
set_milligauss_ratio(_instance[i], 1.0f);
}
// give the driver a chance to run, and gather one sample
hal.scheduler->delay(40);
accumulate();
if (_count[0] == 0) {
hal.console->printf("Failed initial compass accumulate\n");
}
return true;
}
bool AP_Compass_UAVCAN::register_uavcan_compass(uint8_t mgr, uint8_t node)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
_manager = mgr;
if (ap_uavcan->register_mag_listener_to_node(this, node)) {
_instance = register_compass();
struct DeviceStructure {
uint8_t bus_type : 3;
uint8_t bus: 5;
uint8_t address;
uint8_t devtype;
};
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
union DeviceId d;
d.devid_s.bus_type = 3;
d.devid_s.bus = mgr;
d.devid_s.address = node;
d.devid_s.devtype = 1;
set_dev_id(_instance, d.devid);
set_external(_instance, true);
_sum.zero();
_count = 0;
accumulate();
debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r");
return true;
}
}
}
return false;
}
static struct device_t * compass_hmc5883l_probe(struct driver_t * drv, struct dtnode_t * n)
{
struct compass_hmc5883l_pdata_t * pdat;
struct compass_t * c;
struct device_t * dev;
struct i2c_device_t * i2cdev;
u8_t ida, idb, idc;
i2cdev = i2c_device_alloc(dt_read_string(n, "i2c-bus", NULL), dt_read_int(n, "slave-address", 0x1e), 0);
if(!i2cdev)
return NULL;
if(hmc5883l_read(i2cdev, REG_IDA, &ida)
&& hmc5883l_read(i2cdev, REG_IDB, &idb)
&& hmc5883l_read(i2cdev, REG_IDC, &idc)
&& (ida == 0x48)
&& (idb == 0x34)
&& (idc == 0x33))
{
hmc5883l_write(i2cdev, REG_CFGA, 0x70);
hmc5883l_write(i2cdev, REG_CFGB, 0x20);
hmc5883l_write(i2cdev, REG_MODE, 0x00);
}
else
{
i2c_device_free(i2cdev);
return NULL;
}
pdat = malloc(sizeof(struct compass_hmc5883l_pdata_t));
if(!pdat)
{
i2c_device_free(i2cdev);
return NULL;
}
c = malloc(sizeof(struct compass_t));
if(!c)
{
i2c_device_free(i2cdev);
free(pdat);
return NULL;
}
pdat->dev = i2cdev;
c->name = alloc_device_name(dt_read_name(n), -1);
c->ox = 0;
c->oy = 0;
c->oz = 0;
c->get = compass_hmc5883l_get;
c->priv = pdat;
if(!register_compass(&dev, c))
{
i2c_device_free(pdat->dev);
free_device_name(c->name);
free(c->priv);
free(c);
return NULL;
}
dev->driver = drv;
return dev;
}
// 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;
}
bool AP_Compass_AK8963::init()
{
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();
// register the compass instance in the frontend
_compass_instance = register_compass();
hal.scheduler->resume_timer_procs();
hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Compass_AK8963::_update));
_start_conversion();
_initialised = true;
return _initialised;
}