ardupilot/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp

275 lines
7.0 KiB
C++

#include <assert.h>
#include <utility>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_Compass_LSM9DS1.h"
#define LSM9DS1M_OFFSET_X_REG_L_M 0x05
#define LSM9DS1M_OFFSET_X_REG_H_M 0x06
#define LSM9DS1M_OFFSET_Y_REG_L_M 0x07
#define LSM9DS1M_OFFSET_Y_REG_H_M 0x08
#define LSM9DS1M_OFFSET_Z_REG_L_M 0x09
#define LSM9DS1M_OFFSET_Z_REG_H_M 0x0A
#define LSM9DS1M_WHO_AM_I 0x0F
#define WHO_AM_I_MAG 0x3D
#define LSM9DS1M_CTRL_REG1_M 0x20
#define LSM9DS1M_TEMP_COMP (0x1 << 7)
#define LSM9DS1M_XY_ULTRA_HIGH (0x3 << 5)
#define LSM9DS1M_80HZ (0x7 << 2)
#define LSM9DS1M_FAST_ODR (0x1 << 1)
#define LSM9DS1M_CTRL_REG2_M 0x21
#define LSM9DS1M_FS_16G (0x3 << 5)
#define LSM9DS1M_CTRL_REG3_M 0x22
#define LSM9DS1M_SPI_ENABLE (0x01 << 2)
#define LSM9DS1M_CONTINUOUS_MODE 0x0
#define LSM9DS1M_CTRL_REG4_M 0x23
#define LSM9DS1M_Z_ULTRA_HIGH (0x3 << 2)
#define LSM9DS1M_CTRL_REG5_M 0x24
#define LSM9DS1M_BDU (0x0 << 6)
#define LSM9DS1M_STATUS_REG_M 0x27
#define LSM9DS1M_OUT_X_L_M 0x28
#define LSM9DS1M_OUT_X_H_M 0x29
#define LSM9DS1M_OUT_Y_L_M 0x2A
#define LSM9DS1M_OUT_Y_H_M 0x2B
#define LSM9DS1M_OUT_Z_L_M 0x2C
#define LSM9DS1M_OUT_Z_H_M 0x2D
#define LSM9DS1M_INT_CFG_M 0x30
#define LSM9DS1M_INT_SRC_M 0x31
#define LSM9DS1M_INT_THS_L_M 0x32
#define LSM9DS1M_INT_THS_H_M 0x33
struct PACKED sample_regs {
uint8_t status;
int16_t val[3];
};
extern const AP_HAL::HAL &hal;
AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
, _rotation(rotation)
{
}
AP_Compass_Backend *AP_Compass_LSM9DS1::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev), rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_Compass_LSM9DS1::init()
{
AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("LSM9DS1: Unable to get bus semaphore\n");
return false;
}
if (!_check_id()) {
hal.console->printf("LSM9DS1: Could not check id\n");
goto errout;
}
if (!_configure()) {
hal.console->printf("LSM9DS1: Could not check id\n");
goto errout;
}
if (!_set_scale()) {
hal.console->printf("LSM9DS1: Could not set scale\n");
goto errout;
}
_compass_instance = register_compass();
set_rotation(_compass_instance, _rotation);
_dev->set_device_type(DEVTYPE_LSM9DS1);
set_dev_id(_compass_instance, _dev->get_bus_id());
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void));
bus_sem->give();
return true;
errout:
bus_sem->give();
return false;
}
void AP_Compass_LSM9DS1::_dump_registers()
{
hal.console->printf("LSMDS1 registers\n");
for (uint8_t reg = LSM9DS1M_OFFSET_X_REG_L_M; reg <= LSM9DS1M_INT_THS_H_M; reg++) {
uint8_t v = _register_read(reg);
hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - (LSM9DS1M_OFFSET_X_REG_L_M-1)) % 16 == 0) {
hal.console->printf("\n");
}
}
hal.console->printf("\n");
}
void AP_Compass_LSM9DS1::_update(void)
{
struct sample_regs regs;
Vector3f raw_field;
uint32_t time_us = AP_HAL::micros();
if (!_block_read(LSM9DS1M_STATUS_REG_M, (uint8_t *) &regs, sizeof(regs))) {
return;
}
if (regs.status & 0x80) {
return;
}
raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
return;
}
raw_field *= _scaling;
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _compass_instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, time_us, _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
_sem->give();
}
}
void AP_Compass_LSM9DS1::read()
{
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_sem->give();
return;
}
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
}
bool AP_Compass_LSM9DS1::_check_id(void)
{
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
uint8_t value = _register_read(LSM9DS1M_WHO_AM_I);
if (value != WHO_AM_I_MAG) {
hal.console->printf("LSM9DS1: unexpected WHOAMI 0x%x\n", (unsigned)value);
return false;
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
return true;
}
bool AP_Compass_LSM9DS1::_configure(void)
{
_register_write(LSM9DS1M_CTRL_REG1_M, LSM9DS1M_TEMP_COMP | LSM9DS1M_XY_ULTRA_HIGH | LSM9DS1M_80HZ | LSM9DS1M_FAST_ODR);
_register_write(LSM9DS1M_CTRL_REG2_M, LSM9DS1M_FS_16G);
_register_write(LSM9DS1M_CTRL_REG3_M, LSM9DS1M_CONTINUOUS_MODE);
_register_write(LSM9DS1M_CTRL_REG4_M, LSM9DS1M_Z_ULTRA_HIGH);
_register_write(LSM9DS1M_CTRL_REG5_M, LSM9DS1M_BDU);
return true;
}
bool AP_Compass_LSM9DS1::_set_scale(void)
{
static const uint8_t FS_M_MASK = 0xc;
_register_modify(LSM9DS1M_CTRL_REG2_M, FS_M_MASK, LSM9DS1M_FS_16G);
_scaling = 0.58f;
return true;
}
uint8_t AP_Compass_LSM9DS1::_register_read(uint8_t reg)
{
uint8_t val = 0;
/* set READ bit */
reg |= 0x80;
_dev->read_registers(reg, &val, 1);
return val;
}
bool AP_Compass_LSM9DS1::_block_read(uint8_t reg, uint8_t *buf, uint32_t size)
{
/* set !MS bit */
reg |= 0xc0;
return _dev->read_registers(reg, buf, size);
}
void AP_Compass_LSM9DS1::_register_write(uint8_t reg, uint8_t val)
{
_dev->write_register(reg, val);
}
void AP_Compass_LSM9DS1::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = _register_read(reg);
val &= ~clearbits;
val |= setbits;
_register_write(reg, val);
}