diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
index 014a813976..66713a96aa 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
@@ -1,379 +1,379 @@
/*
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program. If not, see .
-
- -- Adapted from Victor Mayoral Vilches's legacy driver under folder LSM9DS0
-*/
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ * -- Adapted from Victor Mayoral Vilches's legacy driver under folder LSM9DS0
+ */
#include
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
-#include "AP_InertialSensor_LSM9DS0.h"
-#include "../AP_HAL_Linux/GPIO.h"
+ #include "AP_InertialSensor_LSM9DS0.h"
+ #include "../AP_HAL_Linux/GPIO.h"
extern const AP_HAL::HAL& hal;
-#define LSM9DS0_G_WHOAMI 0xD4
-#define LSM9DS0_XM_WHOAMI 0x49
+ #define LSM9DS0_G_WHOAMI 0xD4
+ #define LSM9DS0_XM_WHOAMI 0x49
////////////////////////////
// LSM9DS0 Gyro Registers //
////////////////////////////
-#define WHO_AM_I_G 0x0F
-#define CTRL_REG1_G 0x20
-# define CTRL_REG1_G_DR_95Hz_BW_12500mHz (0x0 << 4)
-# define CTRL_REG1_G_DR_95Hz_BW_25Hz (0x1 << 4)
-# define CTRL_REG1_G_DR_190Hz_BW_12500mHz (0x4 << 4)
-# define CTRL_REG1_G_DR_190Hz_BW_25Hz (0x5 << 4)
-# define CTRL_REG1_G_DR_190Hz_BW_50Hz (0x6 << 4)
-# define CTRL_REG1_G_DR_190Hz_BW_70Hz (0x7 << 4)
-# define CTRL_REG1_G_DR_380Hz_BW_20Hz (0x8 << 4)
-# define CTRL_REG1_G_DR_380Hz_BW_25Hz (0x9 << 4)
-# define CTRL_REG1_G_DR_380Hz_BW_50Hz (0xA << 4)
-# define CTRL_REG1_G_DR_380Hz_BW_100Hz (0xB << 4)
-# define CTRL_REG1_G_DR_760Hz_BW_30Hz (0xC << 4)
-# define CTRL_REG1_G_DR_760Hz_BW_35Hz (0xD << 4)
-# define CTRL_REG1_G_DR_760Hz_BW_50Hz (0xE << 4)
-# define CTRL_REG1_G_DR_760Hz_BW_100Hz (0xF << 4)
-# define CTRL_REG1_G_PD (0x1 << 3)
-# define CTRL_REG1_G_ZEN (0x1 << 2)
-# define CTRL_REG1_G_YEN (0x1 << 1)
-# define CTRL_REG1_G_XEN (0x1 << 0)
-#define CTRL_REG2_G 0x21
-# define CTRL_REG2_G_HPM_NORMAL_RESET (0x0 << 4)
-# define CTRL_REG2_G_HPM_REFERENCE (0x1 << 4)
-# define CTRL_REG2_G_HPM_NORMAL (0x2 << 4)
-# define CTRL_REG2_G_HPM_AUTORESET (0x3 << 4)
-# define CTRL_REG2_G_HPCF_0 (0x0 << 0)
-# define CTRL_REG2_G_HPCF_1 (0x1 << 0)
-# define CTRL_REG2_G_HPCF_2 (0x2 << 0)
-# define CTRL_REG2_G_HPCF_3 (0x3 << 0)
-# define CTRL_REG2_G_HPCF_4 (0x4 << 0)
-# define CTRL_REG2_G_HPCF_5 (0x5 << 0)
-# define CTRL_REG2_G_HPCF_6 (0x6 << 0)
-# define CTRL_REG2_G_HPCF_7 (0x7 << 0)
-# define CTRL_REG2_G_HPCF_8 (0x8 << 0)
-# define CTRL_REG2_G_HPCF_9 (0x9 << 0)
-#define CTRL_REG3_G 0x22
-# define CTRL_REG3_G_I1_INT1 (0x1 << 7)
-# define CTRL_REG3_G_I1_BOOT (0x1 << 6)
-# define CTRL_REG3_G_H_LACTIVE (0x1 << 5)
-# define CTRL_REG3_G_PP_OD (0x1 << 4)
-# define CTRL_REG3_G_I2_DRDY (0x1 << 3)
-# define CTRL_REG3_G_I2_WTM (0x1 << 2)
-# define CTRL_REG3_G_I2_ORUN (0x1 << 1)
-# define CTRL_REG3_G_I2_EMPTY (0x1 << 0)
-#define CTRL_REG4_G 0x23
-# define CTRL_REG4_G_BDU (0x1 << 7)
-# define CTRL_REG4_G_BLE (0x1 << 6)
-# define CTRL_REG4_G_FS_245DPS (0x0 << 4)
-# define CTRL_REG4_G_FS_500DPS (0x1 << 4)
-# define CTRL_REG4_G_FS_2000DPS (0x2 << 4)
-# define CTRL_REG4_G_ST_NORMAL (0x0 << 1)
-# define CTRL_REG4_G_ST_0 (0x1 << 1)
-# define CTRL_REG4_G_ST_1 (0x3 << 1)
-# define CTRL_REG4_G_SIM_3WIRE (0x1 << 0)
-#define CTRL_REG5_G 0x24
-# define CTRL_REG5_G_BOOT (0x1 << 7)
-# define CTRL_REG5_G_FIFO_EN (0x1 << 6)
-# define CTRL_REG5_G_HPEN (0x1 << 4)
-# define CTRL_REG5_G_INT1_SEL_00 (0x0 << 2)
-# define CTRL_REG5_G_INT1_SEL_01 (0x1 << 2)
-# define CTRL_REG5_G_INT1_SEL_10 (0x2 << 2)
-# define CTRL_REG5_G_INT1_SEL_11 (0x3 << 2)
-# define CTRL_REG5_G_OUT_SEL_00 (0x0 << 0)
-# define CTRL_REG5_G_OUT_SEL_01 (0x1 << 0)
-# define CTRL_REG5_G_OUT_SEL_10 (0x2 << 0)
-# define CTRL_REG5_G_OUT_SEL_11 (0x3 << 0)
-#define REFERENCE_G 0x25
-#define STATUS_REG_G 0x27
-# define STATUS_REG_G_ZYXOR (0x1 << 7)
-# define STATUS_REG_G_ZOR (0x1 << 6)
-# define STATUS_REG_G_YOR (0x1 << 5)
-# define STATUS_REG_G_XOR (0x1 << 4)
-# define STATUS_REG_G_ZYXDA (0x1 << 3)
-# define STATUS_REG_G_ZDA (0x1 << 2)
-# define STATUS_REG_G_YDA (0x1 << 1)
-# define STATUS_REG_G_XDA (0x1 << 0)
-#define OUT_X_L_G 0x28
-#define OUT_X_H_G 0x29
-#define OUT_Y_L_G 0x2A
-#define OUT_Y_H_G 0x2B
-#define OUT_Z_L_G 0x2C
-#define OUT_Z_H_G 0x2D
-#define FIFO_CTRL_REG_G 0x2E
-# define FIFO_CTRL_REG_G_FM_BYPASS (0x0 << 5)
-# define FIFO_CTRL_REG_G_FM_FIFO (0x1 << 5)
-# define FIFO_CTRL_REG_G_FM_STREAM (0x2 << 5)
-# define FIFO_CTRL_REG_G_FM_STREAM_TO_FIFO (0x3 << 5)
-# define FIFO_CTRL_REG_G_FM_BYPASS_TO_STREAM (0x4 << 5)
-# define FIFO_CTRL_REG_G_WTM_MASK 0x1F
-#define FIFO_SRC_REG_G 0x2F
-# define FIFO_SRC_REG_G_WTM (0x1 << 7)
-# define FIFO_SRC_REG_G_OVRN (0x1 << 6)
-# define FIFO_SRC_REG_G_EMPTY (0x1 << 5)
-# define FIFO_SRC_REG_G_FSS_MASK 0x1F
-#define INT1_CFG_G 0x30
-# define INT1_CFG_G_AND_OR (0x1 << 7)
-# define INT1_CFG_G_LIR (0x1 << 6)
-# define INT1_CFG_G_ZHIE (0x1 << 5)
-# define INT1_CFG_G_ZLIE (0x1 << 4)
-# define INT1_CFG_G_YHIE (0x1 << 3)
-# define INT1_CFG_G_YLIE (0x1 << 2)
-# define INT1_CFG_G_XHIE (0x1 << 1)
-# define INT1_CFG_G_XLIE (0x1 << 0)
-#define INT1_SRC_G 0x31
-# define INT1_SRC_G_IA (0x1 << 6)
-# define INT1_SRC_G_ZH (0x1 << 5)
-# define INT1_SRC_G_ZL (0x1 << 4)
-# define INT1_SRC_G_YH (0x1 << 3)
-# define INT1_SRC_G_YL (0x1 << 2)
-# define INT1_SRC_G_XH (0x1 << 1)
-# define INT1_SRC_G_XL (0x1 << 0)
-#define INT1_THS_XH_G 0x32
-#define INT1_THS_XL_G 0x33
-#define INT1_THS_YH_G 0x34
-#define INT1_THS_YL_G 0x35
-#define INT1_THS_ZH_G 0x36
-#define INT1_THS_ZL_G 0x37
-#define INT1_DURATION_G 0x38
-# define INT1_DURATION_G_WAIT (0x1 << 7)
-# define INT1_DURATION_G_D_MASK 0x7F
+ #define WHO_AM_I_G 0x0F
+ #define CTRL_REG1_G 0x20
+ # define CTRL_REG1_G_DR_95Hz_BW_12500mHz (0x0 << 4)
+ # define CTRL_REG1_G_DR_95Hz_BW_25Hz (0x1 << 4)
+ # define CTRL_REG1_G_DR_190Hz_BW_12500mHz (0x4 << 4)
+ # define CTRL_REG1_G_DR_190Hz_BW_25Hz (0x5 << 4)
+ # define CTRL_REG1_G_DR_190Hz_BW_50Hz (0x6 << 4)
+ # define CTRL_REG1_G_DR_190Hz_BW_70Hz (0x7 << 4)
+ # define CTRL_REG1_G_DR_380Hz_BW_20Hz (0x8 << 4)
+ # define CTRL_REG1_G_DR_380Hz_BW_25Hz (0x9 << 4)
+ # define CTRL_REG1_G_DR_380Hz_BW_50Hz (0xA << 4)
+ # define CTRL_REG1_G_DR_380Hz_BW_100Hz (0xB << 4)
+ # define CTRL_REG1_G_DR_760Hz_BW_30Hz (0xC << 4)
+ # define CTRL_REG1_G_DR_760Hz_BW_35Hz (0xD << 4)
+ # define CTRL_REG1_G_DR_760Hz_BW_50Hz (0xE << 4)
+ # define CTRL_REG1_G_DR_760Hz_BW_100Hz (0xF << 4)
+ # define CTRL_REG1_G_PD (0x1 << 3)
+ # define CTRL_REG1_G_ZEN (0x1 << 2)
+ # define CTRL_REG1_G_YEN (0x1 << 1)
+ # define CTRL_REG1_G_XEN (0x1 << 0)
+ #define CTRL_REG2_G 0x21
+ # define CTRL_REG2_G_HPM_NORMAL_RESET (0x0 << 4)
+ # define CTRL_REG2_G_HPM_REFERENCE (0x1 << 4)
+ # define CTRL_REG2_G_HPM_NORMAL (0x2 << 4)
+ # define CTRL_REG2_G_HPM_AUTORESET (0x3 << 4)
+ # define CTRL_REG2_G_HPCF_0 (0x0 << 0)
+ # define CTRL_REG2_G_HPCF_1 (0x1 << 0)
+ # define CTRL_REG2_G_HPCF_2 (0x2 << 0)
+ # define CTRL_REG2_G_HPCF_3 (0x3 << 0)
+ # define CTRL_REG2_G_HPCF_4 (0x4 << 0)
+ # define CTRL_REG2_G_HPCF_5 (0x5 << 0)
+ # define CTRL_REG2_G_HPCF_6 (0x6 << 0)
+ # define CTRL_REG2_G_HPCF_7 (0x7 << 0)
+ # define CTRL_REG2_G_HPCF_8 (0x8 << 0)
+ # define CTRL_REG2_G_HPCF_9 (0x9 << 0)
+ #define CTRL_REG3_G 0x22
+ # define CTRL_REG3_G_I1_INT1 (0x1 << 7)
+ # define CTRL_REG3_G_I1_BOOT (0x1 << 6)
+ # define CTRL_REG3_G_H_LACTIVE (0x1 << 5)
+ # define CTRL_REG3_G_PP_OD (0x1 << 4)
+ # define CTRL_REG3_G_I2_DRDY (0x1 << 3)
+ # define CTRL_REG3_G_I2_WTM (0x1 << 2)
+ # define CTRL_REG3_G_I2_ORUN (0x1 << 1)
+ # define CTRL_REG3_G_I2_EMPTY (0x1 << 0)
+ #define CTRL_REG4_G 0x23
+ # define CTRL_REG4_G_BDU (0x1 << 7)
+ # define CTRL_REG4_G_BLE (0x1 << 6)
+ # define CTRL_REG4_G_FS_245DPS (0x0 << 4)
+ # define CTRL_REG4_G_FS_500DPS (0x1 << 4)
+ # define CTRL_REG4_G_FS_2000DPS (0x2 << 4)
+ # define CTRL_REG4_G_ST_NORMAL (0x0 << 1)
+ # define CTRL_REG4_G_ST_0 (0x1 << 1)
+ # define CTRL_REG4_G_ST_1 (0x3 << 1)
+ # define CTRL_REG4_G_SIM_3WIRE (0x1 << 0)
+ #define CTRL_REG5_G 0x24
+ # define CTRL_REG5_G_BOOT (0x1 << 7)
+ # define CTRL_REG5_G_FIFO_EN (0x1 << 6)
+ # define CTRL_REG5_G_HPEN (0x1 << 4)
+ # define CTRL_REG5_G_INT1_SEL_00 (0x0 << 2)
+ # define CTRL_REG5_G_INT1_SEL_01 (0x1 << 2)
+ # define CTRL_REG5_G_INT1_SEL_10 (0x2 << 2)
+ # define CTRL_REG5_G_INT1_SEL_11 (0x3 << 2)
+ # define CTRL_REG5_G_OUT_SEL_00 (0x0 << 0)
+ # define CTRL_REG5_G_OUT_SEL_01 (0x1 << 0)
+ # define CTRL_REG5_G_OUT_SEL_10 (0x2 << 0)
+ # define CTRL_REG5_G_OUT_SEL_11 (0x3 << 0)
+ #define REFERENCE_G 0x25
+ #define STATUS_REG_G 0x27
+ # define STATUS_REG_G_ZYXOR (0x1 << 7)
+ # define STATUS_REG_G_ZOR (0x1 << 6)
+ # define STATUS_REG_G_YOR (0x1 << 5)
+ # define STATUS_REG_G_XOR (0x1 << 4)
+ # define STATUS_REG_G_ZYXDA (0x1 << 3)
+ # define STATUS_REG_G_ZDA (0x1 << 2)
+ # define STATUS_REG_G_YDA (0x1 << 1)
+ # define STATUS_REG_G_XDA (0x1 << 0)
+ #define OUT_X_L_G 0x28
+ #define OUT_X_H_G 0x29
+ #define OUT_Y_L_G 0x2A
+ #define OUT_Y_H_G 0x2B
+ #define OUT_Z_L_G 0x2C
+ #define OUT_Z_H_G 0x2D
+ #define FIFO_CTRL_REG_G 0x2E
+ # define FIFO_CTRL_REG_G_FM_BYPASS (0x0 << 5)
+ # define FIFO_CTRL_REG_G_FM_FIFO (0x1 << 5)
+ # define FIFO_CTRL_REG_G_FM_STREAM (0x2 << 5)
+ # define FIFO_CTRL_REG_G_FM_STREAM_TO_FIFO (0x3 << 5)
+ # define FIFO_CTRL_REG_G_FM_BYPASS_TO_STREAM (0x4 << 5)
+ # define FIFO_CTRL_REG_G_WTM_MASK 0x1F
+ #define FIFO_SRC_REG_G 0x2F
+ # define FIFO_SRC_REG_G_WTM (0x1 << 7)
+ # define FIFO_SRC_REG_G_OVRN (0x1 << 6)
+ # define FIFO_SRC_REG_G_EMPTY (0x1 << 5)
+ # define FIFO_SRC_REG_G_FSS_MASK 0x1F
+ #define INT1_CFG_G 0x30
+ # define INT1_CFG_G_AND_OR (0x1 << 7)
+ # define INT1_CFG_G_LIR (0x1 << 6)
+ # define INT1_CFG_G_ZHIE (0x1 << 5)
+ # define INT1_CFG_G_ZLIE (0x1 << 4)
+ # define INT1_CFG_G_YHIE (0x1 << 3)
+ # define INT1_CFG_G_YLIE (0x1 << 2)
+ # define INT1_CFG_G_XHIE (0x1 << 1)
+ # define INT1_CFG_G_XLIE (0x1 << 0)
+ #define INT1_SRC_G 0x31
+ # define INT1_SRC_G_IA (0x1 << 6)
+ # define INT1_SRC_G_ZH (0x1 << 5)
+ # define INT1_SRC_G_ZL (0x1 << 4)
+ # define INT1_SRC_G_YH (0x1 << 3)
+ # define INT1_SRC_G_YL (0x1 << 2)
+ # define INT1_SRC_G_XH (0x1 << 1)
+ # define INT1_SRC_G_XL (0x1 << 0)
+ #define INT1_THS_XH_G 0x32
+ #define INT1_THS_XL_G 0x33
+ #define INT1_THS_YH_G 0x34
+ #define INT1_THS_YL_G 0x35
+ #define INT1_THS_ZH_G 0x36
+ #define INT1_THS_ZL_G 0x37
+ #define INT1_DURATION_G 0x38
+ # define INT1_DURATION_G_WAIT (0x1 << 7)
+ # define INT1_DURATION_G_D_MASK 0x7F
//////////////////////////////////////////
// LSM9DS0 Accel/Magneto (XM) Registers //
//////////////////////////////////////////
-#define OUT_TEMP_L_XM 0x05
-#define OUT_TEMP_H_XM 0x06
-#define STATUS_REG_M 0x07
-# define STATUS_REG_M_ZYXMOR (0x1 << 7)
-# define STATUS_REG_M_ZMOR (0x1 << 6)
-# define STATUS_REG_M_YMOR (0x1 << 5)
-# define STATUS_REG_M_XMOR (0x1 << 4)
-# define STATUS_REG_M_ZYXMDA (0x1 << 3)
-# define STATUS_REG_M_ZMDA (0x1 << 2)
-# define STATUS_REG_M_YMDA (0x1 << 1)
-# define STATUS_REG_M_XMDA (0x1 << 0)
-#define OUT_X_L_M 0x08
-#define OUT_X_H_M 0x09
-#define OUT_Y_L_M 0x0A
-#define OUT_Y_H_M 0x0B
-#define OUT_Z_L_M 0x0C
-#define OUT_Z_H_M 0x0D
-#define WHO_AM_I_XM 0x0F
-#define INT_CTRL_REG_M 0x12
-# define INT_CTRL_REG_M_XMIEN (0x1 << 7)
-# define INT_CTRL_REG_M_YMIEN (0x1 << 6)
-# define INT_CTRL_REG_M_ZMIEN (0x1 << 5)
-# define INT_CTRL_REG_M_PP_OD (0x1 << 4)
-# define INT_CTRL_REG_M_IEA (0x1 << 3)
-# define INT_CTRL_REG_M_IEL (0x1 << 2)
-# define INT_CTRL_REG_M_4D (0x1 << 1)
-# define INT_CTRL_REG_M_MIEN (0x1 << 0)
-#define INT_SRC_REG_M 0x13
-# define INT_SRC_REG_M_M_PTH_X (0x1 << 7)
-# define INT_SRC_REG_M_M_PTH_Y (0x1 << 6)
-# define INT_SRC_REG_M_M_PTH_Z (0x1 << 5)
-# define INT_SRC_REG_M_M_NTH_X (0x1 << 4)
-# define INT_SRC_REG_M_M_NTH_Y (0x1 << 3)
-# define INT_SRC_REG_M_M_NTH_Z (0x1 << 2)
-# define INT_SRC_REG_M_MROI (0x1 << 1)
-# define INT_SRC_REG_M_MINT (0x1 << 0)
-#define INT_THS_L_M 0x14
-#define INT_THS_H_M 0x15
-#define OFFSET_X_L_M 0x16
-#define OFFSET_X_H_M 0x17
-#define OFFSET_Y_L_M 0x18
-#define OFFSET_Y_H_M 0x19
-#define OFFSET_Z_L_M 0x1A
-#define OFFSET_Z_H_M 0x1B
-#define REFERENCE_X 0x1C
-#define REFERENCE_Y 0x1D
-#define REFERENCE_Z 0x1E
-#define CTRL_REG0_XM 0x1F
-# define CTRL_REG0_XM_B00T (0x1 << 7)
-# define CTRL_REG0_XM_FIFO_EN (0x1 << 6)
-# define CTRL_REG0_XM_WTM_EN (0x1 << 5)
-# define CTRL_REG0_XM_HP_CLICK (0x1 << 2)
-# define CTRL_REG0_XM_HPIS1 (0x1 << 1)
-# define CTRL_REG0_XM_HPIS2 (0x1 << 0)
-#define CTRL_REG1_XM 0x20
-# define CTRL_REG1_XM_AODR_POWERDOWN (0x0 << 4)
-# define CTRL_REG1_XM_AODR_3125mHz (0x1 << 4)
-# define CTRL_REG1_XM_AODR_6250mHz (0x2 << 4)
-# define CTRL_REG1_XM_AODR_12500mHz (0x3 << 4)
-# define CTRL_REG1_XM_AODR_25Hz (0x4 << 4)
-# define CTRL_REG1_XM_AODR_50Hz (0x5 << 4)
-# define CTRL_REG1_XM_AODR_100Hz (0x6 << 4)
-# define CTRL_REG1_XM_AODR_200Hz (0x7 << 4)
-# define CTRL_REG1_XM_AODR_400Hz (0x8 << 4)
-# define CTRL_REG1_XM_AODR_800Hz (0x9 << 4)
-# define CTRL_REG1_XM_AODR_1600Hz (0xA << 4)
-# define CTRL_REG1_XM_BDU (0x1 << 3)
-# define CTRL_REG1_XM_AZEN (0x1 << 2)
-# define CTRL_REG1_XM_AYEN (0x1 << 1)
-# define CTRL_REG1_XM_AXEN (0x1 << 0)
-#define CTRL_REG2_XM 0x21
-# define CTRL_REG2_XM_ABW_773Hz (0x0 << 6)
-# define CTRL_REG2_XM_ABW_194Hz (0x1 << 6)
-# define CTRL_REG2_XM_ABW_362Hz (0x2 << 6)
-# define CTRL_REG2_XM_ABW_50Hz (0x3 << 6)
-# define CTRL_REG2_XM_AFS_2G (0x0 << 3)
-# define CTRL_REG2_XM_AFS_4G (0x1 << 3)
-# define CTRL_REG2_XM_AFS_6G (0x2 << 3)
-# define CTRL_REG2_XM_AFS_8G (0x3 << 3)
-# define CTRL_REG2_XM_AFS_16G (0x4 << 3)
-# define CTRL_REG2_XM_AST_NORMAL (0x0 << 1)
-# define CTRL_REG2_XM_AST_POSITIVE (0x1 << 1)
-# define CTRL_REG2_XM_AST_NEGATIVE (0x2 << 1)
-# define CTRL_REG2_XM_SIM_3WIRE (0x1 << 0)
-#define CTRL_REG3_XM 0x22
-# define CTRL_REG3_XM_P1_BOOT (0x1 << 7)
-# define CTRL_REG3_XM_P1_TAP (0x1 << 6)
-# define CTRL_REG3_XM_P1_INT1 (0x1 << 5)
-# define CTRL_REG3_XM_P1_INT2 (0x1 << 4)
-# define CTRL_REG3_XM_P1_INTM (0x1 << 3)
-# define CTRL_REG3_XM_P1_DRDYA (0x1 << 2)
-# define CTRL_REG3_XM_P1_DRDYM (0x1 << 1)
-# define CTRL_REG3_XM_P1_EMPTY (0x1 << 0)
-#define CTRL_REG4_XM 0x23
-# define CTRL_REG4_XM_P2_TAP (0x1 << 7)
-# define CTRL_REG4_XM_P2_INT1 (0x1 << 6)
-# define CTRL_REG4_XM_P2_INT2 (0x1 << 5)
-# define CTRL_REG4_XM_P2_INTM (0x1 << 4)
-# define CTRL_REG4_XM_P2_DRDYA (0x1 << 3)
-# define CTRL_REG4_XM_P2_DRDYM (0x1 << 2)
-# define CTRL_REG4_XM_P2_OVERRUN (0x1 << 1)
-# define CTRL_REG4_XM_P2_WTM (0x1 << 0)
-#define CTRL_REG5_XM 0x24
-# define CTRL_REG5_XM_TEMP_EN (0x1 << 7)
-# define CTRL_REG5_XM_M_RES_LOW (0x0 << 5)
-# define CTRL_REG5_XM_M_RES_HIGH (0x3 << 5)
-# define CTRL_REG5_XM_ODR_3125mHz (0x0 << 2)
-# define CTRL_REG5_XM_ODR_6250mHz (0x1 << 2)
-# define CTRL_REG5_XM_ODR_12500mHz (0x2 << 2)
-# define CTRL_REG5_XM_ODR_25Hz (0x3 << 2)
-# define CTRL_REG5_XM_ODR_50Hz (0x4 << 2)
-# define CTRL_REG5_XM_ODR_100Hz (0x5 << 2)
-# define CTRL_REG5_XM_LIR2 (0x1 << 1)
-# define CTRL_REG5_XM_LIR1 (0x1 << 0)
-#define CTRL_REG6_XM 0x25
-# define CTRL_REG6_XM_MFS_2Gs (0x0 << 5)
-# define CTRL_REG6_XM_MFS_4Gs (0x1 << 5)
-# define CTRL_REG6_XM_MFS_8Gs (0x2 << 5)
-# define CTRL_REG6_XM_MFS_12Gs (0x3 << 5)
-#define CTRL_REG7_XM 0x26
-# define CTRL_REG7_XM_AHPM_NORMAL_RESET (0x0 << 6)
-# define CTRL_REG7_XM_AHPM_REFERENCE (0x1 << 6)
-# define CTRL_REG7_XM_AHPM_NORMAL (0x2 << 6)
-# define CTRL_REG7_XM_AHPM_AUTORESET (0x3 << 6)
-# define CTRL_REG7_XM_AFDS (0x1 << 5)
-# define CTRL_REG7_XM_MLP (0x1 << 2)
-# define CTRL_REG7_XM_MD_CONTINUOUS (0x0 << 0)
-# define CTRL_REG7_XM_MD_SINGLE (0x1 << 0)
-# define CTRL_REG7_XM_MD_POWERDOWN (0x2 << 0)
-#define STATUS_REG_A 0x27
-# define STATUS_REG_A_ZYXAOR (0x1 << 7)
-# define STATUS_REG_A_ZAOR (0x1 << 6)
-# define STATUS_REG_A_YAOR (0x1 << 5)
-# define STATUS_REG_A_XAOR (0x1 << 4)
-# define STATUS_REG_A_ZYXADA (0x1 << 3)
-# define STATUS_REG_A_ZADA (0x1 << 2)
-# define STATUS_REG_A_YADA (0x1 << 1)
-# define STATUS_REG_A_XADA (0x1 << 0)
-#define OUT_X_L_A 0x28
-#define OUT_X_H_A 0x29
-#define OUT_Y_L_A 0x2A
-#define OUT_Y_H_A 0x2B
-#define OUT_Z_L_A 0x2C
-#define OUT_Z_H_A 0x2D
-#define FIFO_CTRL_REG 0x2E
-# define FIFO_CTRL_REG_FM_BYPASS (0x0 << 5)
-# define FIFO_CTRL_REG_FM_FIFO (0x1 << 5)
-# define FIFO_CTRL_REG_FM_STREAM (0x2 << 5)
-# define FIFO_CTRL_REG_FM_STREAM_TO_FIFO (0x3 << 5)
-# define FIFO_CTRL_REG_FM_BYPASS_TO_STREAM (0x4 << 5)
-# define FIFO_CTRL_REG_FTH_MASK 0x1F
-#define FIFO_SRC_REG 0x2F
-# define FIFO_SRC_REG_WTM (0x1 << 7)
-# define FIFO_SRC_REG_OVRN (0x1 << 6)
-# define FIFO_SRC_REG_EMPTY (0x1 << 5)
-# define FIFO_SRC_REG_FSS_MASK 0x1F
-#define INT_GEN_1_REG 0x30
-# define INT_GEN_1_REG_AOI (0x1 << 7)
-# define INT_GEN_1_REG_6D (0x1 << 6)
-# define INT_GEN_1_REG_ZHIE_ZUPE (0x1 << 5)
-# define INT_GEN_1_REG_ZLIE_ZDOWNE (0x1 << 4)
-# define INT_GEN_1_REG_YHIE_YUPE (0x1 << 3)
-# define INT_GEN_1_REG_YLIE_YDOWNE (0x1 << 2)
-# define INT_GEN_1_REG_XHIE_XUPE (0x1 << 1)
-# define INT_GEN_1_REG_XLIE_XDOWNE (0x1 << 0)
-#define INT_GEN_1_SRC 0x31
-# define INT_GEN_1_SRC_IA (0x1 << 6)
-# define INT_GEN_1_SRC_ZH (0x1 << 5)
-# define INT_GEN_1_SRC_ZL (0x1 << 4)
-# define INT_GEN_1_SRC_YH (0x1 << 3)
-# define INT_GEN_1_SRC_YL (0x1 << 2)
-# define INT_GEN_1_SRC_XH (0x1 << 1)
-# define INT_GEN_1_SRC_XL (0x1 << 0)
-#define INT_GEN_1_THS 0x32
-#define INT_GEN_1_DURATION 0x33
-#define INT_GEN_2_REG 0x34
-# define INT_GEN_2_REG_AOI (0x1 << 7)
-# define INT_GEN_2_REG_6D (0x1 << 6)
-# define INT_GEN_2_REG_ZHIE_ZUPE (0x1 << 5)
-# define INT_GEN_2_REG_ZLIE_ZDOWNE (0x1 << 4)
-# define INT_GEN_2_REG_YHIE_YUPE (0x1 << 3)
-# define INT_GEN_2_REG_YLIE_YDOWNE (0x1 << 2)
-# define INT_GEN_2_REG_XHIE_XUPE (0x1 << 1)
-# define INT_GEN_2_REG_XLIE_XDOWNE (0x1 << 0)
-#define INT_GEN_2_SRC 0x35
-# define INT_GEN_2_SRC_IA (0x1 << 6)
-# define INT_GEN_2_SRC_ZH (0x1 << 5)
-# define INT_GEN_2_SRC_ZL (0x1 << 4)
-# define INT_GEN_2_SRC_YH (0x1 << 3)
-# define INT_GEN_2_SRC_YL (0x1 << 2)
-# define INT_GEN_2_SRC_XH (0x1 << 1)
-# define INT_GEN_2_SRC_XL (0x1 << 0)
-#define INT_GEN_2_THS 0x36
-#define INT_GEN_2_DURATION 0x37
-#define CLICK_CFG 0x38
-# define CLICK_CFG_ZD (0x1 << 5)
-# define CLICK_CFG_ZS (0x1 << 4)
-# define CLICK_CFG_YD (0x1 << 3)
-# define CLICK_CFG_YS (0x1 << 2)
-# define CLICK_CFG_XD (0x1 << 1)
-# define CLICK_CFG_XS (0x1 << 0)
-#define CLICK_SRC 0x39
-# define CLICK_SRC_IA (0x1 << 6)
-# define CLICK_SRC_DCLICK (0x1 << 5)
-# define CLICK_SRC_SCLICK (0x1 << 4)
-# define CLICK_SRC_SIGN (0x1 << 3)
-# define CLICK_SRC_Z (0x1 << 2)
-# define CLICK_SRC_Y (0x1 << 1)
-# define CLICK_SRC_X (0x1 << 0)
-#define CLICK_THS 0x3A
-#define TIME_LIMIT 0x3B
-#define TIME_LATENCY 0x3C
-#define TIME_WINDOW 0x3D
-#define ACT_THS 0x3E
-#define ACT_DUR 0x3F
+ #define OUT_TEMP_L_XM 0x05
+ #define OUT_TEMP_H_XM 0x06
+ #define STATUS_REG_M 0x07
+ # define STATUS_REG_M_ZYXMOR (0x1 << 7)
+ # define STATUS_REG_M_ZMOR (0x1 << 6)
+ # define STATUS_REG_M_YMOR (0x1 << 5)
+ # define STATUS_REG_M_XMOR (0x1 << 4)
+ # define STATUS_REG_M_ZYXMDA (0x1 << 3)
+ # define STATUS_REG_M_ZMDA (0x1 << 2)
+ # define STATUS_REG_M_YMDA (0x1 << 1)
+ # define STATUS_REG_M_XMDA (0x1 << 0)
+ #define OUT_X_L_M 0x08
+ #define OUT_X_H_M 0x09
+ #define OUT_Y_L_M 0x0A
+ #define OUT_Y_H_M 0x0B
+ #define OUT_Z_L_M 0x0C
+ #define OUT_Z_H_M 0x0D
+ #define WHO_AM_I_XM 0x0F
+ #define INT_CTRL_REG_M 0x12
+ # define INT_CTRL_REG_M_XMIEN (0x1 << 7)
+ # define INT_CTRL_REG_M_YMIEN (0x1 << 6)
+ # define INT_CTRL_REG_M_ZMIEN (0x1 << 5)
+ # define INT_CTRL_REG_M_PP_OD (0x1 << 4)
+ # define INT_CTRL_REG_M_IEA (0x1 << 3)
+ # define INT_CTRL_REG_M_IEL (0x1 << 2)
+ # define INT_CTRL_REG_M_4D (0x1 << 1)
+ # define INT_CTRL_REG_M_MIEN (0x1 << 0)
+ #define INT_SRC_REG_M 0x13
+ # define INT_SRC_REG_M_M_PTH_X (0x1 << 7)
+ # define INT_SRC_REG_M_M_PTH_Y (0x1 << 6)
+ # define INT_SRC_REG_M_M_PTH_Z (0x1 << 5)
+ # define INT_SRC_REG_M_M_NTH_X (0x1 << 4)
+ # define INT_SRC_REG_M_M_NTH_Y (0x1 << 3)
+ # define INT_SRC_REG_M_M_NTH_Z (0x1 << 2)
+ # define INT_SRC_REG_M_MROI (0x1 << 1)
+ # define INT_SRC_REG_M_MINT (0x1 << 0)
+ #define INT_THS_L_M 0x14
+ #define INT_THS_H_M 0x15
+ #define OFFSET_X_L_M 0x16
+ #define OFFSET_X_H_M 0x17
+ #define OFFSET_Y_L_M 0x18
+ #define OFFSET_Y_H_M 0x19
+ #define OFFSET_Z_L_M 0x1A
+ #define OFFSET_Z_H_M 0x1B
+ #define REFERENCE_X 0x1C
+ #define REFERENCE_Y 0x1D
+ #define REFERENCE_Z 0x1E
+ #define CTRL_REG0_XM 0x1F
+ # define CTRL_REG0_XM_B00T (0x1 << 7)
+ # define CTRL_REG0_XM_FIFO_EN (0x1 << 6)
+ # define CTRL_REG0_XM_WTM_EN (0x1 << 5)
+ # define CTRL_REG0_XM_HP_CLICK (0x1 << 2)
+ # define CTRL_REG0_XM_HPIS1 (0x1 << 1)
+ # define CTRL_REG0_XM_HPIS2 (0x1 << 0)
+ #define CTRL_REG1_XM 0x20
+ # define CTRL_REG1_XM_AODR_POWERDOWN (0x0 << 4)
+ # define CTRL_REG1_XM_AODR_3125mHz (0x1 << 4)
+ # define CTRL_REG1_XM_AODR_6250mHz (0x2 << 4)
+ # define CTRL_REG1_XM_AODR_12500mHz (0x3 << 4)
+ # define CTRL_REG1_XM_AODR_25Hz (0x4 << 4)
+ # define CTRL_REG1_XM_AODR_50Hz (0x5 << 4)
+ # define CTRL_REG1_XM_AODR_100Hz (0x6 << 4)
+ # define CTRL_REG1_XM_AODR_200Hz (0x7 << 4)
+ # define CTRL_REG1_XM_AODR_400Hz (0x8 << 4)
+ # define CTRL_REG1_XM_AODR_800Hz (0x9 << 4)
+ # define CTRL_REG1_XM_AODR_1600Hz (0xA << 4)
+ # define CTRL_REG1_XM_BDU (0x1 << 3)
+ # define CTRL_REG1_XM_AZEN (0x1 << 2)
+ # define CTRL_REG1_XM_AYEN (0x1 << 1)
+ # define CTRL_REG1_XM_AXEN (0x1 << 0)
+ #define CTRL_REG2_XM 0x21
+ # define CTRL_REG2_XM_ABW_773Hz (0x0 << 6)
+ # define CTRL_REG2_XM_ABW_194Hz (0x1 << 6)
+ # define CTRL_REG2_XM_ABW_362Hz (0x2 << 6)
+ # define CTRL_REG2_XM_ABW_50Hz (0x3 << 6)
+ # define CTRL_REG2_XM_AFS_2G (0x0 << 3)
+ # define CTRL_REG2_XM_AFS_4G (0x1 << 3)
+ # define CTRL_REG2_XM_AFS_6G (0x2 << 3)
+ # define CTRL_REG2_XM_AFS_8G (0x3 << 3)
+ # define CTRL_REG2_XM_AFS_16G (0x4 << 3)
+ # define CTRL_REG2_XM_AST_NORMAL (0x0 << 1)
+ # define CTRL_REG2_XM_AST_POSITIVE (0x1 << 1)
+ # define CTRL_REG2_XM_AST_NEGATIVE (0x2 << 1)
+ # define CTRL_REG2_XM_SIM_3WIRE (0x1 << 0)
+ #define CTRL_REG3_XM 0x22
+ # define CTRL_REG3_XM_P1_BOOT (0x1 << 7)
+ # define CTRL_REG3_XM_P1_TAP (0x1 << 6)
+ # define CTRL_REG3_XM_P1_INT1 (0x1 << 5)
+ # define CTRL_REG3_XM_P1_INT2 (0x1 << 4)
+ # define CTRL_REG3_XM_P1_INTM (0x1 << 3)
+ # define CTRL_REG3_XM_P1_DRDYA (0x1 << 2)
+ # define CTRL_REG3_XM_P1_DRDYM (0x1 << 1)
+ # define CTRL_REG3_XM_P1_EMPTY (0x1 << 0)
+ #define CTRL_REG4_XM 0x23
+ # define CTRL_REG4_XM_P2_TAP (0x1 << 7)
+ # define CTRL_REG4_XM_P2_INT1 (0x1 << 6)
+ # define CTRL_REG4_XM_P2_INT2 (0x1 << 5)
+ # define CTRL_REG4_XM_P2_INTM (0x1 << 4)
+ # define CTRL_REG4_XM_P2_DRDYA (0x1 << 3)
+ # define CTRL_REG4_XM_P2_DRDYM (0x1 << 2)
+ # define CTRL_REG4_XM_P2_OVERRUN (0x1 << 1)
+ # define CTRL_REG4_XM_P2_WTM (0x1 << 0)
+ #define CTRL_REG5_XM 0x24
+ # define CTRL_REG5_XM_TEMP_EN (0x1 << 7)
+ # define CTRL_REG5_XM_M_RES_LOW (0x0 << 5)
+ # define CTRL_REG5_XM_M_RES_HIGH (0x3 << 5)
+ # define CTRL_REG5_XM_ODR_3125mHz (0x0 << 2)
+ # define CTRL_REG5_XM_ODR_6250mHz (0x1 << 2)
+ # define CTRL_REG5_XM_ODR_12500mHz (0x2 << 2)
+ # define CTRL_REG5_XM_ODR_25Hz (0x3 << 2)
+ # define CTRL_REG5_XM_ODR_50Hz (0x4 << 2)
+ # define CTRL_REG5_XM_ODR_100Hz (0x5 << 2)
+ # define CTRL_REG5_XM_LIR2 (0x1 << 1)
+ # define CTRL_REG5_XM_LIR1 (0x1 << 0)
+ #define CTRL_REG6_XM 0x25
+ # define CTRL_REG6_XM_MFS_2Gs (0x0 << 5)
+ # define CTRL_REG6_XM_MFS_4Gs (0x1 << 5)
+ # define CTRL_REG6_XM_MFS_8Gs (0x2 << 5)
+ # define CTRL_REG6_XM_MFS_12Gs (0x3 << 5)
+ #define CTRL_REG7_XM 0x26
+ # define CTRL_REG7_XM_AHPM_NORMAL_RESET (0x0 << 6)
+ # define CTRL_REG7_XM_AHPM_REFERENCE (0x1 << 6)
+ # define CTRL_REG7_XM_AHPM_NORMAL (0x2 << 6)
+ # define CTRL_REG7_XM_AHPM_AUTORESET (0x3 << 6)
+ # define CTRL_REG7_XM_AFDS (0x1 << 5)
+ # define CTRL_REG7_XM_MLP (0x1 << 2)
+ # define CTRL_REG7_XM_MD_CONTINUOUS (0x0 << 0)
+ # define CTRL_REG7_XM_MD_SINGLE (0x1 << 0)
+ # define CTRL_REG7_XM_MD_POWERDOWN (0x2 << 0)
+ #define STATUS_REG_A 0x27
+ # define STATUS_REG_A_ZYXAOR (0x1 << 7)
+ # define STATUS_REG_A_ZAOR (0x1 << 6)
+ # define STATUS_REG_A_YAOR (0x1 << 5)
+ # define STATUS_REG_A_XAOR (0x1 << 4)
+ # define STATUS_REG_A_ZYXADA (0x1 << 3)
+ # define STATUS_REG_A_ZADA (0x1 << 2)
+ # define STATUS_REG_A_YADA (0x1 << 1)
+ # define STATUS_REG_A_XADA (0x1 << 0)
+ #define OUT_X_L_A 0x28
+ #define OUT_X_H_A 0x29
+ #define OUT_Y_L_A 0x2A
+ #define OUT_Y_H_A 0x2B
+ #define OUT_Z_L_A 0x2C
+ #define OUT_Z_H_A 0x2D
+ #define FIFO_CTRL_REG 0x2E
+ # define FIFO_CTRL_REG_FM_BYPASS (0x0 << 5)
+ # define FIFO_CTRL_REG_FM_FIFO (0x1 << 5)
+ # define FIFO_CTRL_REG_FM_STREAM (0x2 << 5)
+ # define FIFO_CTRL_REG_FM_STREAM_TO_FIFO (0x3 << 5)
+ # define FIFO_CTRL_REG_FM_BYPASS_TO_STREAM (0x4 << 5)
+ # define FIFO_CTRL_REG_FTH_MASK 0x1F
+ #define FIFO_SRC_REG 0x2F
+ # define FIFO_SRC_REG_WTM (0x1 << 7)
+ # define FIFO_SRC_REG_OVRN (0x1 << 6)
+ # define FIFO_SRC_REG_EMPTY (0x1 << 5)
+ # define FIFO_SRC_REG_FSS_MASK 0x1F
+ #define INT_GEN_1_REG 0x30
+ # define INT_GEN_1_REG_AOI (0x1 << 7)
+ # define INT_GEN_1_REG_6D (0x1 << 6)
+ # define INT_GEN_1_REG_ZHIE_ZUPE (0x1 << 5)
+ # define INT_GEN_1_REG_ZLIE_ZDOWNE (0x1 << 4)
+ # define INT_GEN_1_REG_YHIE_YUPE (0x1 << 3)
+ # define INT_GEN_1_REG_YLIE_YDOWNE (0x1 << 2)
+ # define INT_GEN_1_REG_XHIE_XUPE (0x1 << 1)
+ # define INT_GEN_1_REG_XLIE_XDOWNE (0x1 << 0)
+ #define INT_GEN_1_SRC 0x31
+ # define INT_GEN_1_SRC_IA (0x1 << 6)
+ # define INT_GEN_1_SRC_ZH (0x1 << 5)
+ # define INT_GEN_1_SRC_ZL (0x1 << 4)
+ # define INT_GEN_1_SRC_YH (0x1 << 3)
+ # define INT_GEN_1_SRC_YL (0x1 << 2)
+ # define INT_GEN_1_SRC_XH (0x1 << 1)
+ # define INT_GEN_1_SRC_XL (0x1 << 0)
+ #define INT_GEN_1_THS 0x32
+ #define INT_GEN_1_DURATION 0x33
+ #define INT_GEN_2_REG 0x34
+ # define INT_GEN_2_REG_AOI (0x1 << 7)
+ # define INT_GEN_2_REG_6D (0x1 << 6)
+ # define INT_GEN_2_REG_ZHIE_ZUPE (0x1 << 5)
+ # define INT_GEN_2_REG_ZLIE_ZDOWNE (0x1 << 4)
+ # define INT_GEN_2_REG_YHIE_YUPE (0x1 << 3)
+ # define INT_GEN_2_REG_YLIE_YDOWNE (0x1 << 2)
+ # define INT_GEN_2_REG_XHIE_XUPE (0x1 << 1)
+ # define INT_GEN_2_REG_XLIE_XDOWNE (0x1 << 0)
+ #define INT_GEN_2_SRC 0x35
+ # define INT_GEN_2_SRC_IA (0x1 << 6)
+ # define INT_GEN_2_SRC_ZH (0x1 << 5)
+ # define INT_GEN_2_SRC_ZL (0x1 << 4)
+ # define INT_GEN_2_SRC_YH (0x1 << 3)
+ # define INT_GEN_2_SRC_YL (0x1 << 2)
+ # define INT_GEN_2_SRC_XH (0x1 << 1)
+ # define INT_GEN_2_SRC_XL (0x1 << 0)
+ #define INT_GEN_2_THS 0x36
+ #define INT_GEN_2_DURATION 0x37
+ #define CLICK_CFG 0x38
+ # define CLICK_CFG_ZD (0x1 << 5)
+ # define CLICK_CFG_ZS (0x1 << 4)
+ # define CLICK_CFG_YD (0x1 << 3)
+ # define CLICK_CFG_YS (0x1 << 2)
+ # define CLICK_CFG_XD (0x1 << 1)
+ # define CLICK_CFG_XS (0x1 << 0)
+ #define CLICK_SRC 0x39
+ # define CLICK_SRC_IA (0x1 << 6)
+ # define CLICK_SRC_DCLICK (0x1 << 5)
+ # define CLICK_SRC_SCLICK (0x1 << 4)
+ # define CLICK_SRC_SIGN (0x1 << 3)
+ # define CLICK_SRC_Z (0x1 << 2)
+ # define CLICK_SRC_Y (0x1 << 1)
+ # define CLICK_SRC_X (0x1 << 0)
+ #define CLICK_THS 0x3A
+ #define TIME_LIMIT 0x3B
+ #define TIME_LATENCY 0x3C
+ #define TIME_WINDOW 0x3D
+ #define ACT_THS 0x3E
+ #define ACT_DUR 0x3F
AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
- int drdy_pin_num_a,
- int drdy_pin_num_g):
- AP_InertialSensor_Backend(imu),
+ int drdy_pin_num_a,
+ int drdy_pin_num_g) :
+ AP_InertialSensor_Backend(imu),
_drdy_pin_a(NULL),
_drdy_pin_g(NULL),
_last_accel_filter_hz(-1),
@@ -393,7 +393,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::detect(AP_InertialSensor &
int drdy_pin_num_a = -1, drdy_pin_num_g = -1;
AP_InertialSensor_LSM9DS0 *sensor =
- new AP_InertialSensor_LSM9DS0(_imu, drdy_pin_num_a, drdy_pin_num_g);
+ new AP_InertialSensor_LSM9DS0(_imu, drdy_pin_num_a, drdy_pin_num_g);
if (sensor == NULL) {
return NULL;
@@ -485,9 +485,9 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor()
_set_accel_max_abs_offset(_accel_instance, Vector3f(5.0f, 5.0f, 5.0f));
-#if LSM9DS0_DEBUG
+ #if LSM9DS0_DEBUG
_dump_registers();
-#endif
+ #endif
/* start the timer process to read samples */
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void));
@@ -568,11 +568,11 @@ void AP_InertialSensor_LSM9DS0::_register_write_g(uint8_t reg, uint8_t val)
void AP_InertialSensor_LSM9DS0::_gyro_init()
{
_register_write_g(CTRL_REG1_G,
- CTRL_REG1_G_DR_760Hz_BW_50Hz |
- CTRL_REG1_G_PD |
- CTRL_REG1_G_ZEN |
- CTRL_REG1_G_YEN |
- CTRL_REG1_G_XEN);
+ CTRL_REG1_G_DR_760Hz_BW_50Hz |
+ CTRL_REG1_G_PD |
+ CTRL_REG1_G_ZEN |
+ CTRL_REG1_G_YEN |
+ CTRL_REG1_G_XEN);
hal.scheduler->delay(1);
_register_write_g(CTRL_REG2_G, 0x00);
@@ -585,8 +585,8 @@ void AP_InertialSensor_LSM9DS0::_gyro_init()
hal.scheduler->delay(1);
_register_write_g(CTRL_REG4_G,
- CTRL_REG4_G_BDU |
- CTRL_REG4_G_FS_2000DPS);
+ CTRL_REG4_G_BDU |
+ CTRL_REG4_G_FS_2000DPS);
_set_gyro_scale(G_SCALE_2000DPS);
hal.scheduler->delay(1);
@@ -600,16 +600,16 @@ void AP_InertialSensor_LSM9DS0::_accel_init()
hal.scheduler->delay(1);
_register_write_xm(CTRL_REG1_XM,
- CTRL_REG1_XM_AODR_800Hz |
- CTRL_REG1_XM_BDU |
- CTRL_REG1_XM_AZEN |
- CTRL_REG1_XM_AYEN |
- CTRL_REG1_XM_AXEN);
+ CTRL_REG1_XM_AODR_800Hz |
+ CTRL_REG1_XM_BDU |
+ CTRL_REG1_XM_AZEN |
+ CTRL_REG1_XM_AYEN |
+ CTRL_REG1_XM_AXEN);
hal.scheduler->delay(1);
_register_write_xm(CTRL_REG2_XM,
- CTRL_REG2_XM_ABW_50Hz |
- CTRL_REG2_XM_AFS_16G);
+ CTRL_REG2_XM_ABW_50Hz |
+ CTRL_REG2_XM_AFS_16G);
_set_accel_scale(A_SCALE_16G);
hal.scheduler->delay(1);
@@ -744,7 +744,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
}
/*
- read from the data registers and update filtered data
+ * read from the data registers and update filtered data
*/
void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
{
@@ -784,7 +784,7 @@ bool AP_InertialSensor_LSM9DS0::update()
}
/*
- set the accel filter frequency
+ * set the accel filter frequency
*/
void AP_InertialSensor_LSM9DS0::_set_accel_filter(uint8_t filter_hz)
{
@@ -792,14 +792,14 @@ void AP_InertialSensor_LSM9DS0::_set_accel_filter(uint8_t filter_hz)
}
/*
- set the gyro filter frequency
+ * set the gyro filter frequency
*/
void AP_InertialSensor_LSM9DS0::_set_gyro_filter(uint8_t filter_hz)
{
_gyro_filter.set_cutoff_frequency(760, filter_hz);
}
-#if LSM9DS0_DEBUG
+ #if LSM9DS0_DEBUG
/* dump all config registers - used for debug */
void AP_InertialSensor_LSM9DS0::_dump_registers(void)
{
@@ -827,6 +827,6 @@ void AP_InertialSensor_LSM9DS0::_dump_registers(void)
hal.console->println();
}
-#endif
+ #endif
#endif /* CONFIG_HAL_BOARD == HAL_BOARD_LINUX */
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
index f4fa240dea..bf7b3f8779 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
@@ -30,24 +30,28 @@ public:
AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
int drdy_pin_num_a, int drdy_pin_num_b);
- bool update();
+ bool update();
- bool gyro_sample_available() { return _gyro_sample_available; };
+ bool gyro_sample_available() {
+ return _gyro_sample_available;
+ };
- bool accel_sample_available() { return _accel_sample_available; };
+ bool accel_sample_available() {
+ return _accel_sample_available;
+ };
- static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
+ static AP_InertialSensor_Backend * detect(AP_InertialSensor &imu);
private:
- struct PACKED sensor_raw_data {
+ struct PACKED sensor_raw_data {
int16_t x;
int16_t y;
int16_t z;
};
- AP_HAL::SPIDeviceDriver *_accel_spi;
- AP_HAL::SPIDeviceDriver *_gyro_spi;
- AP_HAL::Semaphore *_spi_sem;
+ AP_HAL::SPIDeviceDriver * _accel_spi;
+ AP_HAL::SPIDeviceDriver * _gyro_spi;
+ AP_HAL::Semaphore * _spi_sem;
/*
* If data-ready GPIO pins numbers are not defined (i.e. any negative
@@ -55,61 +59,61 @@ private:
* by reading the status register. It is *strongly* recommended to use
* data-ready GPIO pins for performance reasons.
*/
- int _drdy_pin_num_a;
- AP_HAL::DigitalSource *_drdy_pin_a;
- int _drdy_pin_num_g;
- AP_HAL::DigitalSource *_drdy_pin_g;
+ int _drdy_pin_num_a;
+ AP_HAL::DigitalSource * _drdy_pin_a;
+ int _drdy_pin_num_g;
+ AP_HAL::DigitalSource * _drdy_pin_g;
- bool _gyro_sample_available;
- bool _accel_sample_available;
+ bool _gyro_sample_available;
+ bool _accel_sample_available;
- bool _accel_data_ready();
- bool _gyro_data_ready();
+ bool _accel_data_ready();
+ bool _gyro_data_ready();
- void _poll_data();
+ void _poll_data();
- bool _init_sensor();
- bool _hardware_init();
+ bool _init_sensor();
+ bool _hardware_init();
- uint8_t _gyro_instance;
- uint8_t _accel_instance;
+ uint8_t _gyro_instance;
+ uint8_t _accel_instance;
- void _gyro_init();
- void _accel_init();
+ void _gyro_init();
+ void _accel_init();
- float _gyro_scale, _accel_scale;
- void _set_gyro_scale(gyro_scale scale);
- void _set_accel_scale(accel_scale scale);
+ float _gyro_scale, _accel_scale;
+ void _set_gyro_scale(gyro_scale scale);
+ void _set_accel_scale(accel_scale scale);
- uint8_t _register_read_xm( uint8_t reg );
- uint8_t _register_read_g( uint8_t reg );
+ uint8_t _register_read_xm( uint8_t reg );
+ uint8_t _register_read_g( uint8_t reg );
- void _register_write_xm( uint8_t reg, uint8_t val );
- void _register_write_g( uint8_t reg, uint8_t val );
+ void _register_write_xm( uint8_t reg, uint8_t val );
+ void _register_write_g( uint8_t reg, uint8_t val );
- void _accel_raw_data(struct sensor_raw_data *raw_data);
- void _gyro_raw_data(struct sensor_raw_data *raw_data);
+ void _accel_raw_data(struct sensor_raw_data *raw_data);
+ void _gyro_raw_data(struct sensor_raw_data *raw_data);
- void _read_data_transaction_a();
- void _read_data_transaction_g();
+ void _read_data_transaction_a();
+ void _read_data_transaction_g();
/* support for updating filter at runtime */
- int16_t _last_gyro_filter_hz;
- int16_t _last_accel_filter_hz;
+ int16_t _last_gyro_filter_hz;
+ int16_t _last_accel_filter_hz;
/* change the filter frequency */
- void _set_accel_filter(uint8_t filter_hz);
- void _set_gyro_filter(uint8_t filter_hz);
+ void _set_accel_filter(uint8_t filter_hz);
+ void _set_gyro_filter(uint8_t filter_hz);
- Vector3f _accel_filtered;
- Vector3f _gyro_filtered;
+ Vector3f _accel_filtered;
+ Vector3f _gyro_filtered;
/* Low Pass filters for gyro and accel */
- LowPassFilter2pVector3f _accel_filter;
- LowPassFilter2pVector3f _gyro_filter;
+ LowPassFilter2pVector3f _accel_filter;
+ LowPassFilter2pVector3f _gyro_filter;
#if LSM9DS0_DEBUG
- void _dump_registers();
+ void _dump_registers();
#endif
};