aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: add LSM9DS1 support

Browse Source 
Make LSM9DS1 driver
This commit is contained in:
Igor Anokhin 2017-10-26 17:31:07 +03:00 committed by Lucas De Marchi
parent 1103e92884
commit 44d5112488
3 changed files with 557 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -89,10 +89,12 @@ public:
DEVTYPE_ACC_MPU6000 = 0x13, DEVTYPE_ACC_MPU6000 = 0x13,
DEVTYPE_ACC_MPU9250 = 0x16, DEVTYPE_ACC_MPU9250 = 0x16,
DEVTYPE_ACC_IIS328DQ = 0x17, DEVTYPE_ACC_IIS328DQ = 0x17,
DEVTYPE_ACC_LSM9DS1 = 0x18,
DEVTYPE_GYR_MPU6000 = 0x21, DEVTYPE_GYR_MPU6000 = 0x21,
DEVTYPE_GYR_L3GD20 = 0x22, DEVTYPE_GYR_L3GD20 = 0x22,
DEVTYPE_GYR_MPU9250 = 0x24, DEVTYPE_GYR_MPU9250 = 0x24,
DEVTYPE_GYR_I3G4250D = 0x25, DEVTYPE_GYR_I3G4250D = 0x25,
DEVTYPE_GYR_LSM9DS1 = 0x26,
}; };
protected: protected:

479
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp Normal file
View File

@ -0,0 +1,479 @@
/*
* This program is free software
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_LSM9DS1.h"
#include <utility>
#include <AP_HAL/GPIO.h>
extern const AP_HAL::HAL& hal;
#define WHO_AM_I 0x68
#define WHO_AM_I_M 0x3D
#define LSM9DS1_DRY_XG_PIN -1
/*
* Accelerometer and Gyroscope registers
*/
#define LSM9DS1XG_ACT_THS 0x04
# define LSM9DS1XG_ACT_THS_SLEEP_ON (0x1 << 7)
#define LSM9DS1XG_ACT_DUR 0x05
#define LSM9DS1XG_INT_GEN_CFG_XL 0x06
# define LSM9DS1XG_INT_GEN_CFG_XL_AOI_XL (0x1 << 7)
# define LSM9DS1XG_INT_GEN_CFG_XL_6D (0x1 << 6)
# define LSM9DS1XG_INT_GEN_CFG_XL_ZHIE_XL (0x1 << 5)
# define LSM9DS1XG_INT_GEN_CFG_XL_ZLIE_XL (0x1 << 4)
# define LSM9DS1XG_INT_GEN_CFG_XL_YHIE_XL (0x1 << 3)
# define LSM9DS1XG_INT_GEN_CFG_XL_YLIE_XL (0x1 << 2)
# define LSM9DS1XG_INT_GEN_CFG_XL_XHIE_XL (0x1 << 1)
# define LSM9DS1XG_INT_GEN_CFG_XL_XLIE_XL (0x1 << 0)
#define LSM9DS1XG_INT_GEN_THS_X_XL 0x07
#define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08
#define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09
#define LSM9DS1XG_INT_GEN_DUR_XL 0x0A
# define LSM9DS1XG_INT_GEN_DUR_XL_WAIT_XL (0x1 << 7)
#define LSM9DS1XG_REFERENCE_G 0x0B
#define LSM9DS1XG_INT1_CTRL 0x0C
# define LSM9DS1XG_INT1_CTRL_INT1_IG_G (0x1 << 7)
# define LSM9DS1XG_INT1_CTRL_INT_IG_XL (0x1 << 6)
# define LSM9DS1XG_INT1_CTRL_INT_FSS5 (0x1 << 5)
# define LSM9DS1XG_INT1_CTRL_INT_OVR (0x1 << 4)
# define LSM9DS1XG_INT1_CTRL_INT_FTH (0x1 << 3)
# define LSM9DS1XG_INT1_CTRL_INT_Boot (0x1 << 2)
# define LSM9DS1XG_INT1_CTRL_INT_DRDY_G (0x1 << 1)
# define LSM9DS1XG_INT1_CTRL_INT_DRDY_XL (0x1 << 0)
#define LSM9DS1XG_INT2_CTRL 0x0D
# define LSM9DS1XG_INT2_CTRL_INT2_INACT (0x1 << 7)
# define LSM9DS1XG_INT2_CTRL_INT2_FSS5 (0x1 << 5)
# define LSM9DS1XG_INT2_CTRL_INT2_OVR (0x1 << 4)
# define LSM9DS1XG_INT2_CTRL_INT2_FTH (0x1 << 3)
# define LSM9DS1XG_INT2_CTRL_INT2_DRDY_TEMP (0x1 << 2)
# define LSM9DS1XG_INT2_CTRL_INT2_DRDY_G (0x1 << 1)
# define LSM9DS1XG_INT2_CTRL_INT2_DRDY_XL (0x1 << 0)
#define LSM9DS1XG_WHO_AM_I 0x0F
#define LSM9DS1XG_CTRL_REG1_G 0x10
# define LSM9DS1XG_CTRL_REG1_G_ODR_G_14900mHz (0x1 << 5)
# define LSM9DS1XG_CTRL_REG1_G_ODR_G_59500mHz (0x2 << 5)
# define LSM9DS1XG_CTRL_REG1_G_ODR_G_119Hz (0x3 << 5)
# define LSM9DS1XG_CTRL_REG1_G_ODR_G_238Hz (0x4 << 5)
# define LSM9DS1XG_CTRL_REG1_G_ODR_G_476Hz (0x5 << 5)
# define LSM9DS1XG_CTRL_REG1_G_ODR_G_952Hz (0x6 << 5)
# define LSM9DS1XG_CTRL_REG1_FS_G_245DPS (0x0 << 3)
# define LSM9DS1XG_CTRL_REG1_FS_G_500DPS (0x1 << 3)
# define LSM9DS1XG_CTRL_REG1_FS_G_2000DPS (0x3 << 3)
#define LSM9DS1XG_CTRL_REG2_G 0x11
# define LSM9DS1XG_CTRL_REG2_G_INT_SEL_00 (0x0 << 2)
# define LSM9DS1XG_CTRL_REG2_G_INT_SEL_01 (0x1 << 2)
# define LSM9DS1XG_CTRL_REG2_G_INT_SEL_10 (0x2 << 2)
# define LSM9DS1XG_CTRL_REG2_G_INT_SEL_11 (0x3 << 2)
# define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_00 (0x0 << 0)
# define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_01 (0x1 << 0)
# define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_10 (0x2 << 0)
# define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_11 (0x3 << 0)
#define LSM9DS1XG_CTRL_REG3_G 0x12
# define LSM9DS1XG_CTRL_REG3_G_LP_MODE (0x1 << 7)
# define LSM9DS1XG_CTRL_REG3_G_HP_EN (0x1 << 6)
#define LSM9DS1XG_ORIENT_CFG_G 0x13
#define LSM9DS1XG_INT_GEN_SRC_G 0x14
# define LSM9DS1XG_INT_GEN_SRC_G_IA_G (0x1 << 6)
# define LSM9DS1XG_INT_GEN_SRC_G_ZH_G (0x1 << 5)
# define LSM9DS1XG_INT_GEN_SRC_G_ZL_G (0x1 << 4)
# define LSM9DS1XG_INT_GEN_SRC_G_YH_G (0x1 << 3)
# define LSM9DS1XG_INT_GEN_SRC_G_YL_G (0x1 << 2)
# define LSM9DS1XG_INT_GEN_SRC_G_XH_G (0x1 << 1)
# define LSM9DS1XG_INT_GEN_SRC_G_XL_G (0x1 << 0)
#define LSM9DS1XG_OUT_TEMP_L 0x15
#define LSM9DS1XG_OUT_TEMP_H 0x16
#define LSM9DS1XG_STATUS_REG 0x17
# define LSM9DS1XG_STATUS_REG_IG_XL (0x1 << 6)
# define LSM9DS1XG_STATUS_REG_IG_G (0x1 << 5)
# define LSM9DS1XG_STATUS_REG_INACT (0x1 << 4)
# define LSM9DS1XG_STATUS_REG_BOOT_STATUS (0x1 << 3)
# define LSM9DS1XG_STATUS_REG_TDA (0x1 << 2)
# define LSM9DS1XG_STATUS_REG_GDA (0x1 << 1)
# define LSM9DS1XG_STATUS_REG_XLDA (0x1 << 0)
#define LSM9DS1XG_OUT_X_L_G 0x18
#define LSM9DS1XG_OUT_X_H_G 0x19
#define LSM9DS1XG_OUT_Y_L_G 0x1A
#define LSM9DS1XG_OUT_Y_H_G 0x1B
#define LSM9DS1XG_OUT_Z_L_G 0x1C
#define LSM9DS1XG_OUT_Z_H_G 0x1D
#define LSM9DS1XG_CTRL_REG4 0x1E
# define LSM9DS1XG_CTRL_REG4_Zen_G (0x1 << 5)
# define LSM9DS1XG_CTRL_REG4_Yen_G (0x1 << 4)
# define LSM9DS1XG_CTRL_REG4_Xen_G (0x1 << 3)
# define LSM9DS1XG_CTRL_REG4_LIR_XL1 (0x1 << 1)
# define LSM9DS1XG_CTRL_REG4_4D_XL1 (0x1 << 0)
#define LSM9DS1XG_CTRL_REG5_XL 0x1F
# define LSM9DS1XG_CTRL_REG5_XL_Zen_XL (0x1 << 5)
# define LSM9DS1XG_CTRL_REG5_XL_Yen_XL (0x1 << 4)
# define LSM9DS1XG_CTRL_REG5_XL_Xen_XL (0x1 << 3)
#define LSM9DS1XG_CTRL_REG6_XL 0x20
# define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_10Hz (0x1 << 5)
# define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_50Hz (0x2 << 5)
# define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_119Hz (0x3 << 5)
# define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_238Hz (0x4 << 5)
# define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_476Hz (0x5 << 5)
# define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_952Hz (0x6 << 5)
# define LSM9DS1XG_CTRL_REG6_XL_FS_XL_2G (0x0 << 3)
# define LSM9DS1XG_CTRL_REG6_XL_FS_XL_16G (0x1 << 3)
# define LSM9DS1XG_CTRL_REG6_XL_FS_XL_4G (0x2 << 3)
# define LSM9DS1XG_CTRL_REG6_XL_FS_XL_8G (0x3 << 3)
# define LSM9DS1XG_CTRL_REG6_XL_BW_SCAL_ODR (0x1 << 2)
# define LSM9DS1XG_CTRL_REG6_XL_BW_XL_408Hz (0x0 << 0)
# define LSM9DS1XG_CTRL_REG6_XL_BW_XL_211Hz (0x1 << 0)
# define LSM9DS1XG_CTRL_REG6_XL_BW_XL_105Hz (0x2 << 0)
# define LSM9DS1XG_CTRL_REG6_XL_BW_XL_50Hz (0x3 << 0)
#define LSM9DS1XG_CTRL_REG7_XL 0x21
# define LSM9DS1XG_CTRL_REG7_XL_FDS (0x1 << 2)
# define LSM9DS1XG_CTRL_REG7_XL_HPIS1 (0x1 << 0)
#define LSM9DS1XG_CTRL_REG8 0x22
# define LSM9DS1XG_CTRL_REG8_BOOT (0x1 << 7)
# define LSM9DS1XG_CTRL_REG8_BDU (0x1 << 6)
# define LSM9DS1XG_CTRL_REG8_H_LACTIVE (0x1 << 5)
# define LSM9DS1XG_CTRL_REG8_PP_OD (0x1 << 4)
# define LSM9DS1XG_CTRL_REG8_SIM (0x1 << 3)
# define LSM9DS1XG_CTRL_REG8_IF_ADD_INC (0x1 << 2)
# define LSM9DS1XG_CTRL_REG8_BLE (0x1 << 1)
# define LSM9DS1XG_CTRL_REG8_SW_RESET (0x1 << 0)
#define LSM9DS1XG_CTRL_REG9 0x23
# define LSM9DS1XG_CTRL_REG9_SLEEP_G (0x1 << 6)
# define LSM9DS1XG_CTRL_REG9_FIFO_TEMP_EN (0x1 << 4)
# define LSM9DS1XG_CTRL_REG9_DRDY_mask_bit (0x1 << 3)
# define LSM9DS1XG_CTRL_REG9_I2C_DISABLE (0x1 << 2)
# define LSM9DS1XG_CTRL_REG9_FIFO_EN (0x1 << 1)
# define LSM9DS1XG_CTRL_REG9_STOP_ON_FTH (0x1 << 0)
#define LSM9DS1XG_CTRL_REG10 0x24
# define LSM9DS1XG_CTRL_REG10_ST_G (0x1 << 2)
# define LSM9DS1XG_CTRL_REG10_ST_XL (0x1 << 0)
#define LSM9DS1XG_INT_GEN_SRC_XL 0x26
# define LSM9DS1XG_INT_GEN_SRC_XL_IA_XL (0x1 << 6)
# define LSM9DS1XG_INT_GEN_SRC_XL_ZH_XL (0x1 << 5)
# define LSM9DS1XG_INT_GEN_SRC_XL_ZL_XL (0x1 << 4)
# define LSM9DS1XG_INT_GEN_SRC_XL_YH_XL (0x1 << 3)
# define LSM9DS1XG_INT_GEN_SRC_XL_YL_XL (0x1 << 2)
# define LSM9DS1XG_INT_GEN_SRC_XL_XH_XL (0x1 << 1)
# define LSM9DS1XG_INT_GEN_SRC_XL_XL_XL (0x1 << 0)
//#define LSM9DS1XG_STATUS_REG 0x27 //repeat
#define LSM9DS1XG_OUT_X_L_XL 0x28
#define LSM9DS1XG_OUT_X_H_XL 0x29
#define LSM9DS1XG_OUT_Y_L_XL 0x2A
#define LSM9DS1XG_OUT_Y_H_XL 0x2B
#define LSM9DS1XG_OUT_Z_L_XL 0x2C
#define LSM9DS1XG_OUT_Z_H_XL 0x2D
#define LSM9DS1XG_FIFO_CTRL 0x2E
# define LSM9DS1XG_FIFO_CTRL_FMODE_BYPASS (0x0 << 5)
# define LSM9DS1XG_FIFO_CTRL_FMODE_FIFO (0x1 << 5)
# define LSM9DS1XG_FIFO_CTRL_FMODE_STREAM (0x3 << 5)
# define LSM9DS1XG_FIFO_CTRL_FMODE_B_TO_S (0x4 << 5)
# define LSM9DS1XG_FIFO_CTRL_FMODE_CON (0x5 << 5)
#define LSM9DS1XG_FIFO_SRC 0x2F
# define LSM9DS1XG_FIFO_SRC_FTH (0x1 << 7)
# define LSM9DS1XG_FIFO_SRC_OVRN (0x1 << 6)
#define LSM9DS1XG_INT_GEN_CFG_G 0x30
# define LSM9DS1XG_INT_GEN_CFG_G_AOI_G (0x1 << 7)
# define LSM9DS1XG_INT_GEN_CFG_G_LIR_G (0x1 << 6)
# define LSM9DS1XG_INT_GEN_CFG_G_ZHIE_G (0x1 << 5)
# define LSM9DS1XG_INT_GEN_CFG_G_ZLIE_G (0x1 << 4)
# define LSM9DS1XG_INT_GEN_CFG_G_YHIE_G (0x1 << 3)
# define LSM9DS1XG_INT_GEN_CFG_G_YLIE_G (0x1 << 2)
# define LSM9DS1XG_INT_GEN_CFG_G_XHIE_G (0x1 << 1)
# define LSM9DS1XG_INT_GEN_CFG_G_XLIE_G (0x1 << 0)
#define LSM9DS1XG_INT_GEN_THS_XH_G 0x31
# define LSM9DS1XG_INT_GEN_THS_XH_G_DCRM_G (0x1 << 7)
#define LSM9DS1XG_INT_GEN_THS_XL_G 0x32
#define LSM9DS1XG_INT_GEN_THS_YH_G 0x33
#define LSM9DS1XG_INT_GEN_THS_YL_G 0x34
#define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35
#define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36
#define LSM9DS1XG_INT_GEN_DUR_G 0x37
# define LSM9DS1XG_INT_GEN_DUR_G_WAIT_G (0x1 << 7)
AP_InertialSensor_LSM9DS1::AP_InertialSensor_LSM9DS1(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
int drdy_pin_num_xg,
enum Rotation rotation)
: AP_InertialSensor_Backend(imu)
, _dev(std::move(dev))
, _drdy_pin_num_xg(drdy_pin_num_xg)
, _rotation(rotation)
{
}
AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS1::probe(AP_InertialSensor &_imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_InertialSensor_LSM9DS1 *sensor =
new AP_InertialSensor_LSM9DS1(_imu,std::move(dev),
LSM9DS1_DRY_XG_PIN,
rotation);
if (!sensor || !sensor->_init_sensor()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_InertialSensor_LSM9DS1::_init_sensor()
{
_spi_sem = _dev->get_semaphore();
if (_drdy_pin_num_xg >= 0) {
_drdy_pin_xg = hal.gpio->channel(_drdy_pin_num_xg);
if (_drdy_pin_xg == nullptr) {
AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel\n");
}
_drdy_pin_xg->mode(HAL_GPIO_INPUT);
}
bool success = _hardware_init();
#if LSM9DS1_DEBUG
_dump_registers();
#endif
return success;
}
bool AP_InertialSensor_LSM9DS1::_hardware_init()
{
if (!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
uint8_t tries, whoami;
// set flag for reading registers
_dev->set_read_flag(0x80);
whoami = _register_read(LSM9DS1XG_WHO_AM_I);
if (whoami != WHO_AM_I) {
hal.console->printf("LSM9DS1: unexpected acc/gyro WHOAMI 0x%x\n", whoami);
goto fail_whoami;
}
for (tries = 0; tries < 5; tries++) {
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
_dev->write_register(LSM9DS1XG_CTRL_REG9,LSM9DS1XG_CTRL_REG9_I2C_DISABLE);
_gyro_init();
_accel_init();
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
hal.scheduler->delay(10);
if (_accel_data_ready() && _gyro_data_ready()) {
break;
}
#if LSM9DS1_DEBUG
_dump_registers();
#endif
}
if (tries == 5) {
hal.console->printf("Failed to boot LSM9DS1 5 times\n\n");
goto fail_tries;
}
_spi_sem->give();
return true;
fail_tries:
fail_whoami:
_spi_sem->give();
return false;
}
/*
start the sensor going
*/
void AP_InertialSensor_LSM9DS1::start(void)
{
_gyro_instance = _imu.register_gyro(952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1));
_accel_instance = _imu.register_accel(952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1));
set_accel_orientation(_accel_instance, _rotation);
set_gyro_orientation(_gyro_instance, _rotation);
_set_accel_max_abs_offset(_accel_instance, 5.0f);
/* start the timer process to read samples */
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS1::_poll_data, void));
}
uint8_t AP_InertialSensor_LSM9DS1::_register_read(uint8_t reg)
{
uint8_t val = 0;
_dev->read_registers(reg, &val, 1);
return val;
}
void AP_InertialSensor_LSM9DS1::_register_write(uint8_t reg, uint8_t val, bool checked)
{
_dev->write_register(reg, val, checked);
}
void AP_InertialSensor_LSM9DS1::_gyro_init()
{
_register_write(LSM9DS1XG_CTRL_REG1_G, LSM9DS1XG_CTRL_REG1_G_ODR_G_952Hz |
LSM9DS1XG_CTRL_REG1_FS_G_2000DPS);
hal.scheduler->delay(1);
_register_write(LSM9DS1XG_CTRL_REG4, LSM9DS1XG_CTRL_REG4_Zen_G |
LSM9DS1XG_CTRL_REG4_Yen_G |
LSM9DS1XG_CTRL_REG4_Xen_G);
_set_gyro_scale();
hal.scheduler->delay(1);
}
void AP_InertialSensor_LSM9DS1::_accel_init()
{
_register_write(LSM9DS1XG_CTRL_REG6_XL, LSM9DS1XG_CTRL_REG6_XL_ODR_XL_952Hz |
LSM9DS1XG_CTRL_REG6_XL_FS_XL_16G);
hal.scheduler->delay(1);
_register_write(LSM9DS1XG_CTRL_REG5_XL, LSM9DS1XG_CTRL_REG5_XL_Zen_XL |
LSM9DS1XG_CTRL_REG5_XL_Yen_XL |
LSM9DS1XG_CTRL_REG5_XL_Xen_XL);
_set_accel_scale(A_SCALE_16G);
hal.scheduler->delay(1);
}
void AP_InertialSensor_LSM9DS1::_set_gyro_scale()
{
/* scale value from datasheet 2000 mdps/digit */
_gyro_scale = 70;
/* convert mdps/digit to dps/digit */
_gyro_scale /= 1000;
/* convert dps/digit to (rad/s)/digit */
_gyro_scale *= DEG_TO_RAD;
}
void AP_InertialSensor_LSM9DS1::_set_accel_scale(accel_scale scale)
{
/*
* Possible accelerometer scales (and their register bit settings) are:
* 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an
* algorithm to calculate g/(ADC tick) based on that 3-bit value:
*/
_accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f;
if (scale == A_SCALE_16G) {
/* the datasheet shows an exception for +-16G */
_accel_scale = 0.000732;
}
/* convert to G/LSB to (m/s/s)/LSB */
_accel_scale *= GRAVITY_MSS;
}
/**
* Timer process to poll for new data from the LSM9DS1.
*/
void AP_InertialSensor_LSM9DS1::_poll_data()
{
if (_gyro_data_ready()) {
_read_data_transaction_g();
}
if (_accel_data_ready()) {
_read_data_transaction_x();
}
// check next register value for correctness
if (!_dev->check_next_register()) {
_inc_accel_error_count(_accel_instance);
}
}
bool AP_InertialSensor_LSM9DS1::_accel_data_ready()
{
if (_drdy_pin_xg != nullptr) {
return _drdy_pin_xg->read() != 0;
}
uint8_t status = _register_read(LSM9DS1XG_STATUS_REG);
return status & LSM9DS1XG_STATUS_REG_XLDA;
}
bool AP_InertialSensor_LSM9DS1::_gyro_data_ready()
{
if (_drdy_pin_xg != nullptr) {
return _drdy_pin_xg->read() != 0;
}
uint8_t status = _register_read(LSM9DS1XG_STATUS_REG);
return status & LSM9DS1XG_STATUS_REG_GDA;
}
void AP_InertialSensor_LSM9DS1::_read_data_transaction_x()
{
struct sensor_raw_data raw_data = { };
const uint8_t reg = LSM9DS1XG_OUT_X_L_XL | 0x80;
if (!_dev->transfer(&reg, 1, (uint8_t *) &raw_data, sizeof(raw_data))) {
hal.console->printf("LSM9DS1: error reading accelerometer\n");
return;
}
Vector3f accel_data(raw_data.x, raw_data.y, -raw_data.z);
accel_data *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data);
}
/*
* read from the data registers and update filtered data
*/
void AP_InertialSensor_LSM9DS1::_read_data_transaction_g()
{
struct sensor_raw_data raw_data = { };
const uint8_t reg = LSM9DS1XG_OUT_X_L_G | 0x80;
if (!_dev->transfer(&reg, 1, (uint8_t *) &raw_data, sizeof(raw_data))) {
hal.console->printf("LSM9DS1: error reading gyroscope\n");
return;
}
Vector3f gyro_data(raw_data.x, raw_data.y, -raw_data.z);
gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
}
bool AP_InertialSensor_LSM9DS1::update()
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
return true;
}
#if LSM9DS1_DEBUG
/*
* dump all config registers - used for debug
*/
void AP_InertialSensor_LSM9DS1::_dump_registers(void)
{
hal.console->println("LSM9DS1 registers:");
const uint8_t first = LSM9DS1XG_ACT_THS;
const uint8_t last = LSM9DS1XG_INT_GEN_DUR_G;
for (uint8_t reg=first; reg<=last; reg++) {
uint8_t v = _register_read(reg);
hal.console->printf("%02x:%02x ", reg, v);
if ((reg - (first-1)) % 16 == 0) {
hal.console->println();
}
}
hal.console->println();
}
#endif

76
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.h Normal file
View File

@ -0,0 +1,76 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/SPIDevice.h>
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
/* enable debug to see a register dump on startup */
#define LSM9DS1_DEBUG 0
class AP_InertialSensor_LSM9DS1 : public AP_InertialSensor_Backend
{
public:
virtual ~AP_InertialSensor_LSM9DS1() { }
void start(void) override;
bool update() override;
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation = ROTATION_NONE);
private:
AP_InertialSensor_LSM9DS1(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
int drdy_pin_num_xg,
enum Rotation rotation);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
struct PACKED sensor_raw_data {
int16_t x;
int16_t y;
int16_t z;
};
enum accel_scale {
A_SCALE_2G = 0,
A_SCALE_4G,
A_SCALE_8G,
A_SCALE_16G
};
bool _accel_data_ready();
bool _gyro_data_ready();
void _poll_data();
bool _init_sensor();
bool _hardware_init();
void _gyro_init();
void _accel_init();
void _set_gyro_scale();
void _set_accel_scale(accel_scale scale);
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
void _read_data_transaction_x();
void _read_data_transaction_g();
#if LSM9DS1_DEBUG
void _dump_registers();
#endif
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
AP_HAL::Semaphore *_spi_sem;
AP_HAL::DigitalSource * _drdy_pin_xg;
float _gyro_scale;
float _accel_scale;
int _drdy_pin_num_xg;
uint8_t _gyro_instance;
uint8_t _accel_instance;
enum Rotation _rotation;
};