You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nifi.apache.org by al...@apache.org on 2018/06/06 15:58:36 UTC
[6/8] nifi-minifi-cpp git commit: MINIFICPP-517: Add RTIMULib and
create basic functionality.
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h
new file mode 100644
index 0000000..967cdaf
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUBNO055.h
@@ -0,0 +1,48 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of
+// this software and associated documentation files (the "Software"), to deal in
+// the Software without restriction, including without limitation the rights to use,
+// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished to do so,
+// subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+#ifndef _RTIMUBNO055_H
+#define _RTIMUBNO055_H
+
+#include "RTIMU.h"
+
+class RTIMUBNO055 : public RTIMU
+{
+public:
+ RTIMUBNO055(RTIMUSettings *settings);
+ ~RTIMUBNO055();
+
+ virtual const char *IMUName() { return "BNO055"; }
+ virtual int IMUType() { return RTIMU_TYPE_BNO055; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ unsigned char m_slaveAddr; // I2C address of BNO055
+
+ uint64_t m_lastReadTime;
+};
+
+#endif // _RTIMUBNO055_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h
new file mode 100644
index 0000000..f0ba1f4
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUDefs.h
@@ -0,0 +1,1119 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of
+// this software and associated documentation files (the "Software"), to deal in
+// the Software without restriction, including without limitation the rights to use,
+// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished to do so,
+// subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+// The MPU-9250 and SPI driver code is based on code generously supplied by
+// staslock@gmail.com (www.clickdrive.io)
+
+
+#ifndef _RTIMUDEFS_H
+#define _RTIMUDEFS_H
+
+// IMU type codes
+//
+// For compatibility, only add new codes at the end to avoid renumbering
+
+#define RTIMU_TYPE_AUTODISCOVER 0 // audodiscover the IMU
+#define RTIMU_TYPE_NULL 1 // if no physical hardware
+#define RTIMU_TYPE_MPU9150 2 // InvenSense MPU9150
+#define RTIMU_TYPE_GD20HM303D 3 // STM L3GD20H/LSM303D (Pololu Altimu)
+#define RTIMU_TYPE_GD20M303DLHC 4 // STM L3GD20/LSM303DHLC (old Adafruit IMU)
+#define RTIMU_TYPE_LSM9DS0 5 // STM LSM9DS0 (eg Sparkfun IMU)
+#define RTIMU_TYPE_LSM9DS1 6 // STM LSM9DS1
+#define RTIMU_TYPE_MPU9250 7 // InvenSense MPU9250
+#define RTIMU_TYPE_GD20HM303DLHC 8 // STM L3GD20H/LSM303DHLC (new Adafruit IMU)
+#define RTIMU_TYPE_BMX055 9 // Bosch BMX055
+#define RTIMU_TYPE_BNO055 10 // Bosch BNO055
+
+//----------------------------------------------------------
+//
+// MPU-9150
+
+// MPU9150 I2C Slave Addresses
+
+#define MPU9150_ADDRESS0 0x68
+#define MPU9150_ADDRESS1 0x69
+#define MPU9150_ID 0x68
+
+// thes magnetometers are on aux bus
+
+#define AK8975_ADDRESS 0x0c
+#define HMC5883_ADDRESS 0x1e
+
+// Register map
+
+#define MPU9150_YG_OFFS_TC 0x01
+#define MPU9150_SMPRT_DIV 0x19
+#define MPU9150_LPF_CONFIG 0x1a
+#define MPU9150_GYRO_CONFIG 0x1b
+#define MPU9150_ACCEL_CONFIG 0x1c
+#define MPU9150_FIFO_EN 0x23
+#define MPU9150_I2C_MST_CTRL 0x24
+#define MPU9150_I2C_SLV0_ADDR 0x25
+#define MPU9150_I2C_SLV0_REG 0x26
+#define MPU9150_I2C_SLV0_CTRL 0x27
+#define MPU9150_I2C_SLV1_ADDR 0x28
+#define MPU9150_I2C_SLV1_REG 0x29
+#define MPU9150_I2C_SLV1_CTRL 0x2a
+#define MPU9150_I2C_SLV4_CTRL 0x34
+#define MPU9150_INT_PIN_CFG 0x37
+#define MPU9150_INT_ENABLE 0x38
+#define MPU9150_INT_STATUS 0x3a
+#define MPU9150_ACCEL_XOUT_H 0x3b
+#define MPU9150_GYRO_XOUT_H 0x43
+#define MPU9150_EXT_SENS_DATA_00 0x49
+#define MPU9150_I2C_SLV1_DO 0x64
+#define MPU9150_I2C_MST_DELAY_CTRL 0x67
+#define MPU9150_USER_CTRL 0x6a
+#define MPU9150_PWR_MGMT_1 0x6b
+#define MPU9150_PWR_MGMT_2 0x6c
+#define MPU9150_FIFO_COUNT_H 0x72
+#define MPU9150_FIFO_R_W 0x74
+#define MPU9150_WHO_AM_I 0x75
+
+// sample rate defines (applies to gyros and accels, not mags)
+
+#define MPU9150_SAMPLERATE_MIN 5 // 5 samples per second is the lowest
+#define MPU9150_SAMPLERATE_MAX 1000 // 1000 samples per second is the absolute maximum
+
+// compass rate defines
+
+#define MPU9150_COMPASSRATE_MIN 1 // 1 samples per second is the lowest
+#define MPU9150_COMPASSRATE_MAX 100 // 100 samples per second is maximum
+
+// LPF options (gyros and accels)
+
+#define MPU9150_LPF_256 0 // gyro: 256Hz, accel: 260Hz
+#define MPU9150_LPF_188 1 // gyro: 188Hz, accel: 184Hz
+#define MPU9150_LPF_98 2 // gyro: 98Hz, accel: 98Hz
+#define MPU9150_LPF_42 3 // gyro: 42Hz, accel: 44Hz
+#define MPU9150_LPF_20 4 // gyro: 20Hz, accel: 21Hz
+#define MPU9150_LPF_10 5 // gyro: 10Hz, accel: 10Hz
+#define MPU9150_LPF_5 6 // gyro: 5Hz, accel: 5Hz
+
+// Gyro FSR options
+
+#define MPU9150_GYROFSR_250 0 // +/- 250 degrees per second
+#define MPU9150_GYROFSR_500 8 // +/- 500 degrees per second
+#define MPU9150_GYROFSR_1000 0x10 // +/- 1000 degrees per second
+#define MPU9150_GYROFSR_2000 0x18 // +/- 2000 degrees per second
+
+// Accel FSR options
+
+#define MPU9150_ACCELFSR_2 0 // +/- 2g
+#define MPU9150_ACCELFSR_4 8 // +/- 4g
+#define MPU9150_ACCELFSR_8 0x10 // +/- 8g
+#define MPU9150_ACCELFSR_16 0x18 // +/- 16g
+
+
+// AK8975 compass registers
+
+#define AK8975_DEVICEID 0x0 // the device ID
+#define AK8975_ST1 0x02 // status 1
+#define AK8975_CNTL 0x0a // control reg
+#define AK8975_ASAX 0x10 // start of the fuse ROM data
+
+// HMC5883 compass registers
+
+#define HMC5883_CONFIG_A 0x00 // configuration A
+#define HMC5883_CONFIG_B 0x01 // configuration B
+#define HMC5883_MODE 0x02 // mode
+#define HMC5883_DATA_X_HI 0x03 // data x msb
+#define HMC5883_STATUS 0x09 // status
+#define HMC5883_ID 0x0a // id
+
+
+//----------------------------------------------------------
+//
+// MPU-9250
+
+// MPU9250 I2C Slave Addresses
+
+#define MPU9250_ADDRESS0 0x68
+#define MPU9250_ADDRESS1 0x69
+#define MPU9250_ID 0x71
+
+#define AK8963_ADDRESS 0x0c
+
+// Register map
+
+#define MPU9250_SMPRT_DIV 0x19
+#define MPU9250_GYRO_LPF 0x1a
+#define MPU9250_GYRO_CONFIG 0x1b
+#define MPU9250_ACCEL_CONFIG 0x1c
+#define MPU9250_ACCEL_LPF 0x1d
+#define MPU9250_FIFO_EN 0x23
+#define MPU9250_I2C_MST_CTRL 0x24
+#define MPU9250_I2C_SLV0_ADDR 0x25
+#define MPU9250_I2C_SLV0_REG 0x26
+#define MPU9250_I2C_SLV0_CTRL 0x27
+#define MPU9250_I2C_SLV1_ADDR 0x28
+#define MPU9250_I2C_SLV1_REG 0x29
+#define MPU9250_I2C_SLV1_CTRL 0x2a
+#define MPU9250_I2C_SLV2_ADDR 0x2b
+#define MPU9250_I2C_SLV2_REG 0x2c
+#define MPU9250_I2C_SLV2_CTRL 0x2d
+#define MPU9250_I2C_SLV4_CTRL 0x34
+#define MPU9250_INT_PIN_CFG 0x37
+#define MPU9250_INT_ENABLE 0x38
+#define MPU9250_INT_STATUS 0x3a
+#define MPU9250_ACCEL_XOUT_H 0x3b
+#define MPU9250_GYRO_XOUT_H 0x43
+#define MPU9250_EXT_SENS_DATA_00 0x49
+#define MPU9250_I2C_SLV1_DO 0x64
+#define MPU9250_I2C_MST_DELAY_CTRL 0x67
+#define MPU9250_USER_CTRL 0x6a
+#define MPU9250_PWR_MGMT_1 0x6b
+#define MPU9250_PWR_MGMT_2 0x6c
+#define MPU9250_FIFO_COUNT_H 0x72
+#define MPU9250_FIFO_R_W 0x74
+#define MPU9250_WHO_AM_I 0x75
+
+// sample rate defines (applies to gyros and accels, not mags)
+
+#define MPU9250_SAMPLERATE_MIN 5 // 5 samples per second is the lowest
+#define MPU9250_SAMPLERATE_MAX 32000 // 32000 samples per second is the absolute maximum
+
+// compass rate defines
+
+#define MPU9250_COMPASSRATE_MIN 1 // 1 samples per second is the lowest
+#define MPU9250_COMPASSRATE_MAX 100 // 100 samples per second is maximum
+
+// Gyro LPF options
+
+#define MPU9250_GYRO_LPF_8800 0x11 // 8800Hz, 0.64mS delay
+#define MPU9250_GYRO_LPF_3600 0x10 // 3600Hz, 0.11mS delay
+#define MPU9250_GYRO_LPF_250 0x00 // 250Hz, 0.97mS delay
+#define MPU9250_GYRO_LPF_184 0x01 // 184Hz, 2.9mS delay
+#define MPU9250_GYRO_LPF_92 0x02 // 92Hz, 3.9mS delay
+#define MPU9250_GYRO_LPF_41 0x03 // 41Hz, 5.9mS delay
+#define MPU9250_GYRO_LPF_20 0x04 // 20Hz, 9.9mS delay
+#define MPU9250_GYRO_LPF_10 0x05 // 10Hz, 17.85mS delay
+#define MPU9250_GYRO_LPF_5 0x06 // 5Hz, 33.48mS delay
+
+// Gyro FSR options
+
+#define MPU9250_GYROFSR_250 0 // +/- 250 degrees per second
+#define MPU9250_GYROFSR_500 8 // +/- 500 degrees per second
+#define MPU9250_GYROFSR_1000 0x10 // +/- 1000 degrees per second
+#define MPU9250_GYROFSR_2000 0x18 // +/- 2000 degrees per second
+
+// Accel FSR options
+
+#define MPU9250_ACCELFSR_2 0 // +/- 2g
+#define MPU9250_ACCELFSR_4 8 // +/- 4g
+#define MPU9250_ACCELFSR_8 0x10 // +/- 8g
+#define MPU9250_ACCELFSR_16 0x18 // +/- 16g
+
+// Accel LPF options
+
+#define MPU9250_ACCEL_LPF_1130 0x08 // 1130Hz, 0.75mS delay
+#define MPU9250_ACCEL_LPF_460 0x00 // 460Hz, 1.94mS delay
+#define MPU9250_ACCEL_LPF_184 0x01 // 184Hz, 5.80mS delay
+#define MPU9250_ACCEL_LPF_92 0x02 // 92Hz, 7.80mS delay
+#define MPU9250_ACCEL_LPF_41 0x03 // 41Hz, 11.80mS delay
+#define MPU9250_ACCEL_LPF_20 0x04 // 20Hz, 19.80mS delay
+#define MPU9250_ACCEL_LPF_10 0x05 // 10Hz, 35.70mS delay
+#define MPU9250_ACCEL_LPF_5 0x06 // 5Hz, 66.96mS delay
+
+// AK8963 compass registers
+
+#define AK8963_DEVICEID 0x48 // the device ID
+#define AK8963_ST1 0x02 // status 1
+#define AK8963_CNTL 0x0a // control reg
+#define AK8963_ASAX 0x10 // start of the fuse ROM data
+
+//----------------------------------------------------------
+//
+// L3GD20
+
+// I2C Slave Addresses
+
+#define L3GD20_ADDRESS0 0x6a
+#define L3GD20_ADDRESS1 0x6b
+#define L3GD20_ID 0xd4
+
+// L3GD20 Register map
+
+#define L3GD20_WHO_AM_I 0x0f
+#define L3GD20_CTRL1 0x20
+#define L3GD20_CTRL2 0x21
+#define L3GD20_CTRL3 0x22
+#define L3GD20_CTRL4 0x23
+#define L3GD20_CTRL5 0x24
+#define L3GD20_OUT_TEMP 0x26
+#define L3GD20_STATUS 0x27
+#define L3GD20_OUT_X_L 0x28
+#define L3GD20_OUT_X_H 0x29
+#define L3GD20_OUT_Y_L 0x2a
+#define L3GD20_OUT_Y_H 0x2b
+#define L3GD20_OUT_Z_L 0x2c
+#define L3GD20_OUT_Z_H 0x2d
+#define L3GD20_FIFO_CTRL 0x2e
+#define L3GD20_FIFO_SRC 0x2f
+#define L3GD20_IG_CFG 0x30
+#define L3GD20_IG_SRC 0x31
+#define L3GD20_IG_THS_XH 0x32
+#define L3GD20_IG_THS_XL 0x33
+#define L3GD20_IG_THS_YH 0x34
+#define L3GD20_IG_THS_YL 0x35
+#define L3GD20_IG_THS_ZH 0x36
+#define L3GD20_IG_THS_ZL 0x37
+#define L3GD20_IG_DURATION 0x38
+
+// Gyro sample rate defines
+
+#define L3GD20_SAMPLERATE_95 0
+#define L3GD20_SAMPLERATE_190 1
+#define L3GD20_SAMPLERATE_380 2
+#define L3GD20_SAMPLERATE_760 3
+
+// Gyro banwidth defines
+
+#define L3GD20_BANDWIDTH_0 0
+#define L3GD20_BANDWIDTH_1 1
+#define L3GD20_BANDWIDTH_2 2
+#define L3GD20_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define L3GD20_FSR_250 0
+#define L3GD20_FSR_500 1
+#define L3GD20_FSR_2000 2
+
+// Gyro high pass filter defines
+
+#define L3GD20_HPF_0 0
+#define L3GD20_HPF_1 1
+#define L3GD20_HPF_2 2
+#define L3GD20_HPF_3 3
+#define L3GD20_HPF_4 4
+#define L3GD20_HPF_5 5
+#define L3GD20_HPF_6 6
+#define L3GD20_HPF_7 7
+#define L3GD20_HPF_8 8
+#define L3GD20_HPF_9 9
+
+//----------------------------------------------------------
+//
+// L3GD20H
+
+// I2C Slave Addresses
+
+#define L3GD20H_ADDRESS0 0x6a
+#define L3GD20H_ADDRESS1 0x6b
+#define L3GD20H_ID 0xd7
+
+// L3GD20H Register map
+
+#define L3GD20H_WHO_AM_I 0x0f
+#define L3GD20H_CTRL1 0x20
+#define L3GD20H_CTRL2 0x21
+#define L3GD20H_CTRL3 0x22
+#define L3GD20H_CTRL4 0x23
+#define L3GD20H_CTRL5 0x24
+#define L3GD20H_OUT_TEMP 0x26
+#define L3GD20H_STATUS 0x27
+#define L3GD20H_OUT_X_L 0x28
+#define L3GD20H_OUT_X_H 0x29
+#define L3GD20H_OUT_Y_L 0x2a
+#define L3GD20H_OUT_Y_H 0x2b
+#define L3GD20H_OUT_Z_L 0x2c
+#define L3GD20H_OUT_Z_H 0x2d
+#define L3GD20H_FIFO_CTRL 0x2e
+#define L3GD20H_FIFO_SRC 0x2f
+#define L3GD20H_IG_CFG 0x30
+#define L3GD20H_IG_SRC 0x31
+#define L3GD20H_IG_THS_XH 0x32
+#define L3GD20H_IG_THS_XL 0x33
+#define L3GD20H_IG_THS_YH 0x34
+#define L3GD20H_IG_THS_YL 0x35
+#define L3GD20H_IG_THS_ZH 0x36
+#define L3GD20H_IG_THS_ZL 0x37
+#define L3GD20H_IG_DURATION 0x38
+#define L3GD20H_LOW_ODR 0x39
+
+// Gyro sample rate defines
+
+#define L3GD20H_SAMPLERATE_12_5 0
+#define L3GD20H_SAMPLERATE_25 1
+#define L3GD20H_SAMPLERATE_50 2
+#define L3GD20H_SAMPLERATE_100 3
+#define L3GD20H_SAMPLERATE_200 4
+#define L3GD20H_SAMPLERATE_400 5
+#define L3GD20H_SAMPLERATE_800 6
+
+// Gyro banwidth defines
+
+#define L3GD20H_BANDWIDTH_0 0
+#define L3GD20H_BANDWIDTH_1 1
+#define L3GD20H_BANDWIDTH_2 2
+#define L3GD20H_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define L3GD20H_FSR_245 0
+#define L3GD20H_FSR_500 1
+#define L3GD20H_FSR_2000 2
+
+// Gyro high pass filter defines
+
+#define L3GD20H_HPF_0 0
+#define L3GD20H_HPF_1 1
+#define L3GD20H_HPF_2 2
+#define L3GD20H_HPF_3 3
+#define L3GD20H_HPF_4 4
+#define L3GD20H_HPF_5 5
+#define L3GD20H_HPF_6 6
+#define L3GD20H_HPF_7 7
+#define L3GD20H_HPF_8 8
+#define L3GD20H_HPF_9 9
+
+//----------------------------------------------------------
+//
+// LSM303D
+
+#define LSM303D_ADDRESS0 0x1e
+#define LSM303D_ADDRESS1 0x1d
+#define LSM303D_ID 0x49
+
+// LSM303D Register Map
+
+#define LSM303D_TEMP_OUT_L 0x05
+#define LSM303D_TEMP_OUT_H 0x06
+#define LSM303D_STATUS_M 0x07
+#define LSM303D_OUT_X_L_M 0x08
+#define LSM303D_OUT_X_H_M 0x09
+#define LSM303D_OUT_Y_L_M 0x0a
+#define LSM303D_OUT_Y_H_M 0x0b
+#define LSM303D_OUT_Z_L_M 0x0c
+#define LSM303D_OUT_Z_H_M 0x0d
+#define LSM303D_WHO_AM_I 0x0f
+#define LSM303D_INT_CTRL_M 0x12
+#define LSM303D_INT_SRC_M 0x13
+#define LSM303D_INT_THS_L_M 0x14
+#define LSM303D_INT_THS_H_M 0x15
+#define LSM303D_OFFSET_X_L_M 0x16
+#define LSM303D_OFFSET_X_H_M 0x17
+#define LSM303D_OFFSET_Y_L_M 0x18
+#define LSM303D_OFFSET_Y_H_M 0x19
+#define LSM303D_OFFSET_Z_L_M 0x1a
+#define LSM303D_OFFSET_Z_H_M 0x1b
+#define LSM303D_REFERENCE_X 0x1c
+#define LSM303D_REFERENCE_Y 0x1d
+#define LSM303D_REFERENCE_Z 0x1e
+#define LSM303D_CTRL0 0x1f
+#define LSM303D_CTRL1 0x20
+#define LSM303D_CTRL2 0x21
+#define LSM303D_CTRL3 0x22
+#define LSM303D_CTRL4 0x23
+#define LSM303D_CTRL5 0x24
+#define LSM303D_CTRL6 0x25
+#define LSM303D_CTRL7 0x26
+#define LSM303D_STATUS_A 0x27
+#define LSM303D_OUT_X_L_A 0x28
+#define LSM303D_OUT_X_H_A 0x29
+#define LSM303D_OUT_Y_L_A 0x2a
+#define LSM303D_OUT_Y_H_A 0x2b
+#define LSM303D_OUT_Z_L_A 0x2c
+#define LSM303D_OUT_Z_H_A 0x2d
+#define LSM303D_FIFO_CTRL 0x2e
+#define LSM303D_FIFO_SRC 0x2f
+#define LSM303D_IG_CFG1 0x30
+#define LSM303D_IG_SRC1 0x31
+#define LSM303D_IG_THS1 0x32
+#define LSM303D_IG_DUR1 0x33
+#define LSM303D_IG_CFG2 0x34
+#define LSM303D_IG_SRC2 0x35
+#define LSM303D_IG_THS2 0x36
+#define LSM303D_IG_DUR2 0x37
+#define LSM303D_CLICK_CFG 0x38
+#define LSM303D_CLICK_SRC 0x39
+#define LSM303D_CLICK_THS 0x3a
+#define LSM303D_TIME_LIMIT 0x3b
+#define LSM303D_TIME_LATENCY 0x3c
+#define LSM303D_TIME_WINDOW 0x3d
+#define LSM303D_ACT_THIS 0x3e
+#define LSM303D_ACT_DUR 0x3f
+
+// Accel sample rate defines
+
+#define LSM303D_ACCEL_SAMPLERATE_3_125 1
+#define LSM303D_ACCEL_SAMPLERATE_6_25 2
+#define LSM303D_ACCEL_SAMPLERATE_12_5 3
+#define LSM303D_ACCEL_SAMPLERATE_25 4
+#define LSM303D_ACCEL_SAMPLERATE_50 5
+#define LSM303D_ACCEL_SAMPLERATE_100 6
+#define LSM303D_ACCEL_SAMPLERATE_200 7
+#define LSM303D_ACCEL_SAMPLERATE_400 8
+#define LSM303D_ACCEL_SAMPLERATE_800 9
+#define LSM303D_ACCEL_SAMPLERATE_1600 10
+
+// Accel FSR
+
+#define LSM303D_ACCEL_FSR_2 0
+#define LSM303D_ACCEL_FSR_4 1
+#define LSM303D_ACCEL_FSR_6 2
+#define LSM303D_ACCEL_FSR_8 3
+#define LSM303D_ACCEL_FSR_16 4
+
+// Accel filter bandwidth
+
+#define LSM303D_ACCEL_LPF_773 0
+#define LSM303D_ACCEL_LPF_194 1
+#define LSM303D_ACCEL_LPF_362 2
+#define LSM303D_ACCEL_LPF_50 3
+
+// Compass sample rate defines
+
+#define LSM303D_COMPASS_SAMPLERATE_3_125 0
+#define LSM303D_COMPASS_SAMPLERATE_6_25 1
+#define LSM303D_COMPASS_SAMPLERATE_12_5 2
+#define LSM303D_COMPASS_SAMPLERATE_25 3
+#define LSM303D_COMPASS_SAMPLERATE_50 4
+#define LSM303D_COMPASS_SAMPLERATE_100 5
+
+// Compass FSR
+
+#define LSM303D_COMPASS_FSR_2 0
+#define LSM303D_COMPASS_FSR_4 1
+#define LSM303D_COMPASS_FSR_8 2
+#define LSM303D_COMPASS_FSR_12 3
+
+//----------------------------------------------------------
+//
+// LSM303DLHC
+
+#define LSM303DLHC_ACCEL_ADDRESS 0x19
+#define LSM303DLHC_COMPASS_ADDRESS 0x1e
+
+// LSM303DLHC Accel Register Map
+
+#define LSM303DLHC_CTRL1_A 0x20
+#define LSM303DLHC_CTRL2_A 0x21
+#define LSM303DLHC_CTRL3_A 0x22
+#define LSM303DLHC_CTRL4_A 0x23
+#define LSM303DLHC_CTRL5_A 0x24
+#define LSM303DLHC_CTRL6_A 0x25
+#define LSM303DLHC_REF_A 0x26
+#define LSM303DLHC_STATUS_A 0x27
+#define LSM303DLHC_OUT_X_L_A 0x28
+#define LSM303DLHC_OUT_X_H_A 0x29
+#define LSM303DLHC_OUT_Y_L_A 0x2a
+#define LSM303DLHC_OUT_Y_H_A 0x2b
+#define LSM303DLHC_OUT_Z_L_A 0x2c
+#define LSM303DLHC_OUT_Z_H_A 0x2d
+#define LSM303DLHC_FIFO_CTRL_A 0x2e
+#define LSM303DLHC_FIFO_SRC_A 0x2f
+
+// LSM303DLHC Compass Register Map
+
+#define LSM303DLHC_CRA_M 0x00
+#define LSM303DLHC_CRB_M 0x01
+#define LSM303DLHC_CRM_M 0x02
+#define LSM303DLHC_OUT_X_H_M 0x03
+#define LSM303DLHC_OUT_X_L_M 0x04
+#define LSM303DLHC_OUT_Y_H_M 0x05
+#define LSM303DLHC_OUT_Y_L_M 0x06
+#define LSM303DLHC_OUT_Z_H_M 0x07
+#define LSM303DLHC_OUT_Z_L_M 0x08
+#define LSM303DLHC_STATUS_M 0x09
+#define LSM303DLHC_TEMP_OUT_L_M 0x31
+#define LSM303DLHC_TEMP_OUT_H_M 0x32
+
+// Accel sample rate defines
+
+#define LSM303DLHC_ACCEL_SAMPLERATE_1 1
+#define LSM303DLHC_ACCEL_SAMPLERATE_10 2
+#define LSM303DLHC_ACCEL_SAMPLERATE_25 3
+#define LSM303DLHC_ACCEL_SAMPLERATE_50 4
+#define LSM303DLHC_ACCEL_SAMPLERATE_100 5
+#define LSM303DLHC_ACCEL_SAMPLERATE_200 6
+#define LSM303DLHC_ACCEL_SAMPLERATE_400 7
+
+// Accel FSR
+
+#define LSM303DLHC_ACCEL_FSR_2 0
+#define LSM303DLHC_ACCEL_FSR_4 1
+#define LSM303DLHC_ACCEL_FSR_8 2
+#define LSM303DLHC_ACCEL_FSR_16 3
+
+// Compass sample rate defines
+
+#define LSM303DLHC_COMPASS_SAMPLERATE_0_75 0
+#define LSM303DLHC_COMPASS_SAMPLERATE_1_5 1
+#define LSM303DLHC_COMPASS_SAMPLERATE_3 2
+#define LSM303DLHC_COMPASS_SAMPLERATE_7_5 3
+#define LSM303DLHC_COMPASS_SAMPLERATE_15 4
+#define LSM303DLHC_COMPASS_SAMPLERATE_30 5
+#define LSM303DLHC_COMPASS_SAMPLERATE_75 6
+#define LSM303DLHC_COMPASS_SAMPLERATE_220 7
+
+// Compass FSR
+
+#define LSM303DLHC_COMPASS_FSR_1_3 1
+#define LSM303DLHC_COMPASS_FSR_1_9 2
+#define LSM303DLHC_COMPASS_FSR_2_5 3
+#define LSM303DLHC_COMPASS_FSR_4 4
+#define LSM303DLHC_COMPASS_FSR_4_7 5
+#define LSM303DLHC_COMPASS_FSR_5_6 6
+#define LSM303DLHC_COMPASS_FSR_8_1 7
+
+//----------------------------------------------------------
+//
+// LSM9DS0
+
+// I2C Slave Addresses
+
+#define LSM9DS0_GYRO_ADDRESS0 0x6a
+#define LSM9DS0_GYRO_ADDRESS1 0x6b
+#define LSM9DS0_GYRO_ID 0xd4
+
+#define LSM9DS0_ACCELMAG_ADDRESS0 0x1e
+#define LSM9DS0_ACCELMAG_ADDRESS1 0x1d
+#define LSM9DS0_ACCELMAG_ID 0x49
+
+// LSM9DS0 Register map
+
+#define LSM9DS0_GYRO_WHO_AM_I 0x0f
+#define LSM9DS0_GYRO_CTRL1 0x20
+#define LSM9DS0_GYRO_CTRL2 0x21
+#define LSM9DS0_GYRO_CTRL3 0x22
+#define LSM9DS0_GYRO_CTRL4 0x23
+#define LSM9DS0_GYRO_CTRL5 0x24
+#define LSM9DS0_GYRO_OUT_TEMP 0x26
+#define LSM9DS0_GYRO_STATUS 0x27
+#define LSM9DS0_GYRO_OUT_X_L 0x28
+#define LSM9DS0_GYRO_OUT_X_H 0x29
+#define LSM9DS0_GYRO_OUT_Y_L 0x2a
+#define LSM9DS0_GYRO_OUT_Y_H 0x2b
+#define LSM9DS0_GYRO_OUT_Z_L 0x2c
+#define LSM9DS0_GYRO_OUT_Z_H 0x2d
+#define LSM9DS0_GYRO_FIFO_CTRL 0x2e
+#define LSM9DS0_GYRO_FIFO_SRC 0x2f
+#define LSM9DS0_GYRO_IG_CFG 0x30
+#define LSM9DS0_GYRO_IG_SRC 0x31
+#define LSM9DS0_GYRO_IG_THS_XH 0x32
+#define LSM9DS0_GYRO_IG_THS_XL 0x33
+#define LSM9DS0_GYRO_IG_THS_YH 0x34
+#define LSM9DS0_GYRO_IG_THS_YL 0x35
+#define LSM9DS0_GYRO_IG_THS_ZH 0x36
+#define LSM9DS0_GYRO_IG_THS_ZL 0x37
+#define LSM9DS0_GYRO_IG_DURATION 0x38
+
+// Gyro sample rate defines
+
+#define LSM9DS0_GYRO_SAMPLERATE_95 0
+#define LSM9DS0_GYRO_SAMPLERATE_190 1
+#define LSM9DS0_GYRO_SAMPLERATE_380 2
+#define LSM9DS0_GYRO_SAMPLERATE_760 3
+
+// Gyro banwidth defines
+
+#define LSM9DS0_GYRO_BANDWIDTH_0 0
+#define LSM9DS0_GYRO_BANDWIDTH_1 1
+#define LSM9DS0_GYRO_BANDWIDTH_2 2
+#define LSM9DS0_GYRO_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define LSM9DS0_GYRO_FSR_250 0
+#define LSM9DS0_GYRO_FSR_500 1
+#define LSM9DS0_GYRO_FSR_2000 2
+
+// Gyro high pass filter defines
+
+#define LSM9DS0_GYRO_HPF_0 0
+#define LSM9DS0_GYRO_HPF_1 1
+#define LSM9DS0_GYRO_HPF_2 2
+#define LSM9DS0_GYRO_HPF_3 3
+#define LSM9DS0_GYRO_HPF_4 4
+#define LSM9DS0_GYRO_HPF_5 5
+#define LSM9DS0_GYRO_HPF_6 6
+#define LSM9DS0_GYRO_HPF_7 7
+#define LSM9DS0_GYRO_HPF_8 8
+#define LSM9DS0_GYRO_HPF_9 9
+
+// Accel/Mag Register Map
+
+#define LSM9DS0_TEMP_OUT_L 0x05
+#define LSM9DS0_TEMP_OUT_H 0x06
+#define LSM9DS0_STATUS_M 0x07
+#define LSM9DS0_OUT_X_L_M 0x08
+#define LSM9DS0_OUT_X_H_M 0x09
+#define LSM9DS0_OUT_Y_L_M 0x0a
+#define LSM9DS0_OUT_Y_H_M 0x0b
+#define LSM9DS0_OUT_Z_L_M 0x0c
+#define LSM9DS0_OUT_Z_H_M 0x0d
+#define LSM9DS0_WHO_AM_I 0x0f
+#define LSM9DS0_INT_CTRL_M 0x12
+#define LSM9DS0_INT_SRC_M 0x13
+#define LSM9DS0_INT_THS_L_M 0x14
+#define LSM9DS0_INT_THS_H_M 0x15
+#define LSM9DS0_OFFSET_X_L_M 0x16
+#define LSM9DS0_OFFSET_X_H_M 0x17
+#define LSM9DS0_OFFSET_Y_L_M 0x18
+#define LSM9DS0_OFFSET_Y_H_M 0x19
+#define LSM9DS0_OFFSET_Z_L_M 0x1a
+#define LSM9DS0_OFFSET_Z_H_M 0x1b
+#define LSM9DS0_REFERENCE_X 0x1c
+#define LSM9DS0_REFERENCE_Y 0x1d
+#define LSM9DS0_REFERENCE_Z 0x1e
+#define LSM9DS0_CTRL0 0x1f
+#define LSM9DS0_CTRL1 0x20
+#define LSM9DS0_CTRL2 0x21
+#define LSM9DS0_CTRL3 0x22
+#define LSM9DS0_CTRL4 0x23
+#define LSM9DS0_CTRL5 0x24
+#define LSM9DS0_CTRL6 0x25
+#define LSM9DS0_CTRL7 0x26
+#define LSM9DS0_STATUS_A 0x27
+#define LSM9DS0_OUT_X_L_A 0x28
+#define LSM9DS0_OUT_X_H_A 0x29
+#define LSM9DS0_OUT_Y_L_A 0x2a
+#define LSM9DS0_OUT_Y_H_A 0x2b
+#define LSM9DS0_OUT_Z_L_A 0x2c
+#define LSM9DS0_OUT_Z_H_A 0x2d
+#define LSM9DS0_FIFO_CTRL 0x2e
+#define LSM9DS0_FIFO_SRC 0x2f
+#define LSM9DS0_IG_CFG1 0x30
+#define LSM9DS0_IG_SRC1 0x31
+#define LSM9DS0_IG_THS1 0x32
+#define LSM9DS0_IG_DUR1 0x33
+#define LSM9DS0_IG_CFG2 0x34
+#define LSM9DS0_IG_SRC2 0x35
+#define LSM9DS0_IG_THS2 0x36
+#define LSM9DS0_IG_DUR2 0x37
+#define LSM9DS0_CLICK_CFG 0x38
+#define LSM9DS0_CLICK_SRC 0x39
+#define LSM9DS0_CLICK_THS 0x3a
+#define LSM9DS0_TIME_LIMIT 0x3b
+#define LSM9DS0_TIME_LATENCY 0x3c
+#define LSM9DS0_TIME_WINDOW 0x3d
+#define LSM9DS0_ACT_THIS 0x3e
+#define LSM9DS0_ACT_DUR 0x3f
+
+// Accel sample rate defines
+
+#define LSM9DS0_ACCEL_SAMPLERATE_3_125 1
+#define LSM9DS0_ACCEL_SAMPLERATE_6_25 2
+#define LSM9DS0_ACCEL_SAMPLERATE_12_5 3
+#define LSM9DS0_ACCEL_SAMPLERATE_25 4
+#define LSM9DS0_ACCEL_SAMPLERATE_50 5
+#define LSM9DS0_ACCEL_SAMPLERATE_100 6
+#define LSM9DS0_ACCEL_SAMPLERATE_200 7
+#define LSM9DS0_ACCEL_SAMPLERATE_400 8
+#define LSM9DS0_ACCEL_SAMPLERATE_800 9
+#define LSM9DS0_ACCEL_SAMPLERATE_1600 10
+
+// Accel FSR
+
+#define LSM9DS0_ACCEL_FSR_2 0
+#define LSM9DS0_ACCEL_FSR_4 1
+#define LSM9DS0_ACCEL_FSR_6 2
+#define LSM9DS0_ACCEL_FSR_8 3
+#define LSM9DS0_ACCEL_FSR_16 4
+
+// Accel filter bandwidth
+
+#define LSM9DS0_ACCEL_LPF_773 0
+#define LSM9DS0_ACCEL_LPF_194 1
+#define LSM9DS0_ACCEL_LPF_362 2
+#define LSM9DS0_ACCEL_LPF_50 3
+
+// Compass sample rate defines
+
+#define LSM9DS0_COMPASS_SAMPLERATE_3_125 0
+#define LSM9DS0_COMPASS_SAMPLERATE_6_25 1
+#define LSM9DS0_COMPASS_SAMPLERATE_12_5 2
+#define LSM9DS0_COMPASS_SAMPLERATE_25 3
+#define LSM9DS0_COMPASS_SAMPLERATE_50 4
+#define LSM9DS0_COMPASS_SAMPLERATE_100 5
+
+// Compass FSR
+
+#define LSM9DS0_COMPASS_FSR_2 0
+#define LSM9DS0_COMPASS_FSR_4 1
+#define LSM9DS0_COMPASS_FSR_8 2
+#define LSM9DS0_COMPASS_FSR_12 3
+
+//----------------------------------------------------------
+//
+// LSM9DS1
+
+// I2C Slave Addresses
+
+#define LSM9DS1_ADDRESS0 0x6a
+#define LSM9DS1_ADDRESS1 0x6b
+#define LSM9DS1_ID 0x68
+
+#define LSM9DS1_MAG_ADDRESS0 0x1c
+#define LSM9DS1_MAG_ADDRESS1 0x1d
+#define LSM9DS1_MAG_ADDRESS2 0x1e
+#define LSM9DS1_MAG_ADDRESS3 0x1f
+#define LSM9DS1_MAG_ID 0x3d
+
+// LSM9DS1 Register map
+
+#define LSM9DS1_ACT_THS 0x04
+#define LSM9DS1_ACT_DUR 0x05
+#define LSM9DS1_INT_GEN_CFG_XL 0x06
+#define LSM9DS1_INT_GEN_THS_X_XL 0x07
+#define LSM9DS1_INT_GEN_THS_Y_XL 0x08
+#define LSM9DS1_INT_GEN_THS_Z_XL 0x09
+#define LSM9DS1_INT_GEN_DUR_XL 0x0A
+#define LSM9DS1_REFERENCE_G 0x0B
+#define LSM9DS1_INT1_CTRL 0x0C
+#define LSM9DS1_INT2_CTRL 0x0D
+#define LSM9DS1_WHO_AM_I 0x0F
+#define LSM9DS1_CTRL1 0x10
+#define LSM9DS1_CTRL2 0x11
+#define LSM9DS1_CTRL3 0x12
+#define LSM9DS1_ORIENT_CFG_G 0x13
+#define LSM9DS1_INT_GEN_SRC_G 0x14
+#define LSM9DS1_OUT_TEMP_L 0x15
+#define LSM9DS1_OUT_TEMP_H 0x16
+#define LSM9DS1_STATUS 0x17
+#define LSM9DS1_OUT_X_L_G 0x18
+#define LSM9DS1_OUT_X_H_G 0x19
+#define LSM9DS1_OUT_Y_L_G 0x1A
+#define LSM9DS1_OUT_Y_H_G 0x1B
+#define LSM9DS1_OUT_Z_L_G 0x1C
+#define LSM9DS1_OUT_Z_H_G 0x1D
+#define LSM9DS1_CTRL4 0x1E
+#define LSM9DS1_CTRL5 0x1F
+#define LSM9DS1_CTRL6 0x20
+#define LSM9DS1_CTRL7 0x21
+#define LSM9DS1_CTRL8 0x22
+#define LSM9DS1_CTRL9 0x23
+#define LSM9DS1_CTRL10 0x24
+#define LSM9DS1_INT_GEN_SRC_XL 0x26
+#define LSM9DS1_STATUS2 0x27
+#define LSM9DS1_OUT_X_L_XL 0x28
+#define LSM9DS1_OUT_X_H_XL 0x29
+#define LSM9DS1_OUT_Y_L_XL 0x2A
+#define LSM9DS1_OUT_Y_H_XL 0x2B
+#define LSM9DS1_OUT_Z_L_XL 0x2C
+#define LSM9DS1_OUT_Z_H_XL 0x2D
+#define LSM9DS1_FIFO_CTRL 0x2E
+#define LSM9DS1_FIFO_SRC 0x2F
+#define LSM9DS1_INT_GEN_CFG_G 0x30
+#define LSM9DS1_INT_GEN_THS_XH_G 0x31
+#define LSM9DS1_INT_GEN_THS_XL_G 0x32
+#define LSM9DS1_INT_GEN_THS_YH_G 0x33
+#define LSM9DS1_INT_GEN_THS_YL_G 0x34
+#define LSM9DS1_INT_GEN_THS_ZH_G 0x35
+#define LSM9DS1_INT_GEN_THS_ZL_G 0x36
+#define LSM9DS1_INT_GEN_DUR_G 0x37
+
+// Gyro sample rate defines
+
+#define LSM9DS1_GYRO_SAMPLERATE_14_9 0
+#define LSM9DS1_GYRO_SAMPLERATE_59_5 1
+#define LSM9DS1_GYRO_SAMPLERATE_119 2
+#define LSM9DS1_GYRO_SAMPLERATE_238 3
+#define LSM9DS1_GYRO_SAMPLERATE_476 4
+#define LSM9DS1_GYRO_SAMPLERATE_952 5
+
+// Gyro banwidth defines
+
+#define LSM9DS1_GYRO_BANDWIDTH_0 0
+#define LSM9DS1_GYRO_BANDWIDTH_1 1
+#define LSM9DS1_GYRO_BANDWIDTH_2 2
+#define LSM9DS1_GYRO_BANDWIDTH_3 3
+
+// Gyro FSR defines
+
+#define LSM9DS1_GYRO_FSR_250 0
+#define LSM9DS1_GYRO_FSR_500 1
+#define LSM9DS1_GYRO_FSR_2000 3
+
+// Gyro high pass filter defines
+
+#define LSM9DS1_GYRO_HPF_0 0
+#define LSM9DS1_GYRO_HPF_1 1
+#define LSM9DS1_GYRO_HPF_2 2
+#define LSM9DS1_GYRO_HPF_3 3
+#define LSM9DS1_GYRO_HPF_4 4
+#define LSM9DS1_GYRO_HPF_5 5
+#define LSM9DS1_GYRO_HPF_6 6
+#define LSM9DS1_GYRO_HPF_7 7
+#define LSM9DS1_GYRO_HPF_8 8
+#define LSM9DS1_GYRO_HPF_9 9
+
+// Mag Register Map
+
+#define LSM9DS1_MAG_OFFSET_X_L 0x05
+#define LSM9DS1_MAG_OFFSET_X_H 0x06
+#define LSM9DS1_MAG_OFFSET_Y_L 0x07
+#define LSM9DS1_MAG_OFFSET_Y_H 0x08
+#define LSM9DS1_MAG_OFFSET_Z_L 0x09
+#define LSM9DS1_MAG_OFFSET_Z_H 0x0A
+#define LSM9DS1_MAG_WHO_AM_I 0x0F
+#define LSM9DS1_MAG_CTRL1 0x20
+#define LSM9DS1_MAG_CTRL2 0x21
+#define LSM9DS1_MAG_CTRL3 0x22
+#define LSM9DS1_MAG_CTRL4 0x23
+#define LSM9DS1_MAG_CTRL5 0x24
+#define LSM9DS1_MAG_STATUS 0x27
+#define LSM9DS1_MAG_OUT_X_L 0x28
+#define LSM9DS1_MAG_OUT_X_H 0x29
+#define LSM9DS1_MAG_OUT_Y_L 0x2A
+#define LSM9DS1_MAG_OUT_Y_H 0x2B
+#define LSM9DS1_MAG_OUT_Z_L 0x2C
+#define LSM9DS1_MAG_OUT_Z_H 0x2D
+#define LSM9DS1_MAG_INT_CFG 0x30
+#define LSM9DS1_MAG_INT_SRC 0x31
+#define LSM9DS1_MAG_INT_THS_L 0x32
+#define LSM9DS1_MAG_INT_THS_H 0x33
+
+// Accel sample rate defines
+
+#define LSM9DS1_ACCEL_SAMPLERATE_14_9 1
+#define LSM9DS1_ACCEL_SAMPLERATE_59_5 2
+#define LSM9DS1_ACCEL_SAMPLERATE_119 3
+#define LSM9DS1_ACCEL_SAMPLERATE_238 4
+#define LSM9DS1_ACCEL_SAMPLERATE_476 5
+#define LSM9DS1_ACCEL_SAMPLERATE_952 6
+
+// Accel FSR
+
+#define LSM9DS1_ACCEL_FSR_2 0
+#define LSM9DS1_ACCEL_FSR_16 1
+#define LSM9DS1_ACCEL_FSR_4 2
+#define LSM9DS1_ACCEL_FSR_8 3
+
+// Accel filter bandwidth
+
+#define LSM9DS1_ACCEL_LPF_408 0
+#define LSM9DS1_ACCEL_LPF_211 1
+#define LSM9DS1_ACCEL_LPF_105 2
+#define LSM9DS1_ACCEL_LPF_50 3
+
+// Compass sample rate defines
+
+#define LSM9DS1_COMPASS_SAMPLERATE_0_625 0
+#define LSM9DS1_COMPASS_SAMPLERATE_1_25 1
+#define LSM9DS1_COMPASS_SAMPLERATE_2_5 2
+#define LSM9DS1_COMPASS_SAMPLERATE_5 3
+#define LSM9DS1_COMPASS_SAMPLERATE_10 4
+#define LSM9DS1_COMPASS_SAMPLERATE_20 5
+#define LSM9DS1_COMPASS_SAMPLERATE_40 6
+#define LSM9DS1_COMPASS_SAMPLERATE_80 7
+
+// Compass FSR
+
+#define LSM9DS1_COMPASS_FSR_4 0
+#define LSM9DS1_COMPASS_FSR_8 1
+#define LSM9DS1_COMPASS_FSR_12 2
+#define LSM9DS1_COMPASS_FSR_16 3
+
+//----------------------------------------------------------
+//
+// BMX055
+
+// I2C Slave Addresses
+
+#define BMX055_GYRO_ADDRESS0 0x68
+#define BMX055_GYRO_ADDRESS1 0x69
+#define BMX055_GYRO_ID 0x0f
+
+#define BMX055_ACCEL_ADDRESS0 0x18
+#define BMX055_ACCEL_ADDRESS1 0x19
+#define BMX055_ACCEL_ID 0xfa
+
+#define BMX055_MAG_ADDRESS0 0x10
+#define BMX055_MAG_ADDRESS1 0x11
+#define BMX055_MAG_ADDRESS2 0x12
+#define BMX055_MAG_ADDRESS3 0x13
+
+#define BMX055_MAG_ID 0x32
+
+// BMX055 Register map
+
+#define BMX055_GYRO_WHO_AM_I 0x00
+#define BMX055_GYRO_X_LSB 0x02
+#define BMX055_GYRO_X_MSB 0x03
+#define BMX055_GYRO_Y_LSB 0x04
+#define BMX055_GYRO_Y_MSB 0x05
+#define BMX055_GYRO_Z_LSB 0x06
+#define BMX055_GYRO_Z_MSB 0x07
+#define BMX055_GYRO_INT_STATUS_0 0x09
+#define BMX055_GYRO_INT_STATUS_1 0x0a
+#define BMX055_GYRO_INT_STATUS_2 0x0b
+#define BMX055_GYRO_INT_STATUS_3 0x0c
+#define BMX055_GYRO_FIFO_STATUS 0x0e
+#define BMX055_GYRO_RANGE 0x0f
+#define BMX055_GYRO_BW 0x10
+#define BMX055_GYRO_LPM1 0x11
+#define BMX055_GYRO_LPM2 0x12
+#define BMX055_GYRO_RATE_HBW 0x13
+#define BMX055_GYRO_SOFT_RESET 0x14
+#define BMX055_GYRO_INT_EN_0 0x15
+#define BMX055_GYRO_1A 0x1a
+#define BMX055_GYRO_1B 0x1b
+#define BMX055_GYRO_SOC 0x31
+#define BMX055_GYRO_FOC 0x32
+#define BMX055_GYRO_FIFO_CONFIG_0 0x3d
+#define BMX055_GYRO_FIFO_CONFIG_1 0x3e
+#define BMX055_GYRO_FIFO_DATA 0x3f
+
+#define BMX055_ACCEL_WHO_AM_I 0x00
+#define BMX055_ACCEL_X_LSB 0x02
+#define BMX055_ACCEL_X_MSB 0x03
+#define BMX055_ACCEL_Y_LSB 0x04
+#define BMX055_ACCEL_Y_MSB 0x05
+#define BMX055_ACCEL_Z_LSB 0x06
+#define BMX055_ACCEL_Z_MSB 0x07
+#define BMX055_ACCEL_TEMP 0x08
+#define BMX055_ACCEL_INT_STATUS_0 0x09
+#define BMX055_ACCEL_INT_STATUS_1 0x0a
+#define BMX055_ACCEL_INT_STATUS_2 0x0b
+#define BMX055_ACCEL_INT_STATUS_3 0x0c
+#define BMX055_ACCEL_FIFO_STATUS 0x0e
+#define BMX055_ACCEL_PMU_RANGE 0x0f
+#define BMX055_ACCEL_PMU_BW 0x10
+#define BMX055_ACCEL_PMU_LPW 0x11
+#define BMX055_ACCEL_PMU_LOW_POWER 0x12
+#define BMX055_ACCEL_HBW 0x13
+#define BMX055_ACCEL_SOFT_RESET 0x14
+#define BMX055_ACCEL_FIFO_CONFIG_0 0x30
+#define BMX055_ACCEL_OFC_CTRL 0x36
+#define BMX055_ACCEL_OFC_SETTING 0x37
+#define BMX055_ACCEL_FIFO_CONFIG_1 0x3e
+#define BMX055_ACCEL_FIFO_DATA 0x3f
+
+#define BMX055_MAG_WHO_AM_I 0x40
+#define BMX055_MAG_X_LSB 0x42
+#define BMX055_MAG_X_MSB 0x43
+#define BMX055_MAG_Y_LSB 0x44
+#define BMX055_MAG_Y_MSB 0x45
+#define BMX055_MAG_Z_LSB 0x46
+#define BMX055_MAG_Z_MSB 0x47
+#define BMX055_MAG_RHALL_LSB 0x48
+#define BMX055_MAG_RHALL_MSB 0x49
+#define BMX055_MAG_INT_STAT 0x4a
+#define BMX055_MAG_POWER 0x4b
+#define BMX055_MAG_MODE 0x4c
+#define BMX055_MAG_INT_ENABLE 0x4d
+#define BMX055_MAG_AXIS_ENABLE 0x4e
+#define BMX055_MAG_REPXY 0x51
+#define BMX055_MAG_REPZ 0x52
+
+#define BMX055_MAG_DIG_X1 0x5D
+#define BMX055_MAG_DIG_Y1 0x5E
+#define BMX055_MAG_DIG_Z4_LSB 0x62
+#define BMX055_MAG_DIG_Z4_MSB 0x63
+#define BMX055_MAG_DIG_X2 0x64
+#define BMX055_MAG_DIG_Y2 0x65
+#define BMX055_MAG_DIG_Z2_LSB 0x68
+#define BMX055_MAG_DIG_Z2_MSB 0x69
+#define BMX055_MAG_DIG_Z1_LSB 0x6A
+#define BMX055_MAG_DIG_Z1_MSB 0x6B
+#define BMX055_MAG_DIG_XYZ1_LSB 0x6C
+#define BMX055_MAG_DIG_XYZ1_MSB 0x6D
+#define BMX055_MAG_DIG_Z3_LSB 0x6E
+#define BMX055_MAG_DIG_Z3_MSB 0x6F
+#define BMX055_MAG_DIG_XY2 0x70
+#define BMX055_MAG_DIG_XY1 0x71
+
+// Gyro sample rate defines
+
+#define BMX055_GYRO_SAMPLERATE_100_32 0x07
+#define BMX055_GYRO_SAMPLERATE_200_64 0x06
+#define BMX055_GYRO_SAMPLERATE_100_12 0x05
+#define BMX055_GYRO_SAMPLERATE_200_23 0x04
+#define BMX055_GYRO_SAMPLERATE_400_47 0x03
+#define BMX055_GYRO_SAMPLERATE_1000_116 0x02
+#define BMX055_GYRO_SAMPLERATE_2000_230 0x01
+#define BMX055_GYRO_SAMPLERATE_2000_523 0x00
+
+// Gyro FSR defines
+
+#define BMX055_GYRO_FSR_2000 0x00
+#define BMX055_GYRO_FSR_1000 0x01
+#define BMX055_GYRO_FSR_500 0x02
+#define BMX055_GYRO_FSR_250 0x03
+#define BMX055_GYRO_FSR_125 0x04
+
+// Accel sample rate defines
+
+#define BMX055_ACCEL_SAMPLERATE_15 0X00
+#define BMX055_ACCEL_SAMPLERATE_31 0X01
+#define BMX055_ACCEL_SAMPLERATE_62 0X02
+#define BMX055_ACCEL_SAMPLERATE_125 0X03
+#define BMX055_ACCEL_SAMPLERATE_250 0X04
+#define BMX055_ACCEL_SAMPLERATE_500 0X05
+#define BMX055_ACCEL_SAMPLERATE_1000 0X06
+#define BMX055_ACCEL_SAMPLERATE_2000 0X07
+
+// Accel FSR defines
+
+#define BMX055_ACCEL_FSR_2 0x00
+#define BMX055_ACCEL_FSR_4 0x01
+#define BMX055_ACCEL_FSR_8 0x02
+#define BMX055_ACCEL_FSR_16 0x03
+
+
+// Mag preset defines
+
+#define BMX055_MAG_LOW_POWER 0x00
+#define BMX055_MAG_REGULAR 0x01
+#define BMX055_MAG_ENHANCED 0x02
+#define BMX055_MAG_HIGH_ACCURACY 0x03
+
+//----------------------------------------------------------
+//
+// BNO055
+
+// I2C Slave Addresses
+
+#define BNO055_ADDRESS0 0x28
+#define BNO055_ADDRESS1 0x29
+#define BNO055_ID 0xa0
+
+// Register map
+
+#define BNO055_WHO_AM_I 0x00
+#define BNO055_PAGE_ID 0x07
+#define BNO055_ACCEL_DATA 0x08
+#define BNO055_MAG_DATA 0x0e
+#define BNO055_GYRO_DATA 0x14
+#define BNO055_FUSED_EULER 0x1a
+#define BNO055_FUSED_QUAT 0x20
+#define BNO055_UNIT_SEL 0x3b
+#define BNO055_OPER_MODE 0x3d
+#define BNO055_PWR_MODE 0x3e
+#define BNO055_SYS_TRIGGER 0x3f
+#define BNO055_AXIS_MAP_CONFIG 0x41
+#define BNO055_AXIS_MAP_SIGN 0x42
+
+// Operation modes
+
+#define BNO055_OPER_MODE_CONFIG 0x00
+#define BNO055_OPER_MODE_NDOF 0x0c
+
+// Power modes
+
+#define BNO055_PWR_MODE_NORMAL 0x00
+
+#endif // _RTIMUDEFS_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp
new file mode 100644
index 0000000..b8e32e6
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.cpp
@@ -0,0 +1,563 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of
+// this software and associated documentation files (the "Software"), to deal in
+// the Software without restriction, including without limitation the rights to use,
+// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished to do so,
+// subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+#include "RTIMUGD20HM303D.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMUGD20HM303D::RTIMUGD20HM303D(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMUGD20HM303D::~RTIMUGD20HM303D()
+{
+}
+
+bool RTIMUGD20HM303D::IMUInit()
+{
+ unsigned char result;
+
+#ifdef GD20HM303D_CACHE_MODE
+ m_firstTime = true;
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+
+ // work out accel/mag address
+
+ if (m_settings->HALRead(LSM303D_ADDRESS0, LSM303D_WHO_AM_I, 1, &result, "")) {
+ if (result == LSM303D_ID) {
+ m_accelCompassSlaveAddr = LSM303D_ADDRESS0;
+ }
+ } else {
+ m_accelCompassSlaveAddr = LSM303D_ADDRESS1;
+ }
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H"))
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id"))
+ return false;
+
+ if (result != L3GD20H_ID) {
+ HAL_ERROR1("Incorrect L3GD20H id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL2())
+ return false;
+
+ if (!setGyroCTRL4())
+ return false;
+
+ // Set up the accel/compass
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, LSM303D_WHO_AM_I, 1, &result, "Failed to read LSM303D id"))
+ return false;
+
+ if (result != LSM303D_ID) {
+ HAL_ERROR1("Incorrect LSM303D id %d\n", result);
+ return false;
+ }
+
+ if (!setAccelCTRL1())
+ return false;
+
+ if (!setAccelCTRL2())
+ return false;
+
+ if (!setCompassCTRL5())
+ return false;
+
+ if (!setCompassCTRL6())
+ return false;
+
+ if (!setCompassCTRL7())
+ return false;
+
+#ifdef GD20HM303D_CACHE_MODE
+
+ // turn on gyro fifo
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20H FIFO mode"))
+ return false;
+#endif
+
+ if (!setGyroCTRL5())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("GD20HM303D init complete\n");
+ return true;
+}
+
+bool RTIMUGD20HM303D::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+ unsigned char lowOdr = 0;
+
+ switch (m_settings->m_GD20HM303DGyroSampleRate) {
+ case L3GD20H_SAMPLERATE_12_5:
+ ctrl1 = 0x0f;
+ lowOdr = 1;
+ m_sampleRate = 13;
+ break;
+
+ case L3GD20H_SAMPLERATE_25:
+ ctrl1 = 0x4f;
+ lowOdr = 1;
+ m_sampleRate = 25;
+ break;
+
+ case L3GD20H_SAMPLERATE_50:
+ ctrl1 = 0x8f;
+ lowOdr = 1;
+ m_sampleRate = 50;
+ break;
+
+ case L3GD20H_SAMPLERATE_100:
+ ctrl1 = 0x0f;
+ m_sampleRate = 100;
+ break;
+
+ case L3GD20H_SAMPLERATE_200:
+ ctrl1 = 0x4f;
+ m_sampleRate = 200;
+ break;
+
+ case L3GD20H_SAMPLERATE_400:
+ ctrl1 = 0x8f;
+ m_sampleRate = 400;
+ break;
+
+ case L3GD20H_SAMPLERATE_800:
+ ctrl1 = 0xcf;
+ m_sampleRate = 800;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H sample rate code %d\n", m_settings->m_GD20HM303DGyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_GD20HM303DGyroBW) {
+ case L3GD20H_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case L3GD20H_BANDWIDTH_1:
+ ctrl1 |= 0x10;
+ break;
+
+ case L3GD20H_BANDWIDTH_2:
+ ctrl1 |= 0x20;
+ break;
+
+ case L3GD20H_BANDWIDTH_3:
+ ctrl1 |= 0x30;
+ break;
+
+ }
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, lowOdr, "Failed to set L3GD20H LOW_ODR"))
+ return false;
+
+ return (m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL1, ctrl1, "Failed to set L3GD20H CTRL1"));
+}
+
+bool RTIMUGD20HM303D::setGyroCTRL2()
+{
+ if ((m_settings->m_GD20HM303DGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DGyroHpf > L3GD20H_HPF_9)) {
+ HAL_ERROR1("Illegal L3GD20H high pass filter code %d\n", m_settings->m_GD20HM303DGyroHpf);
+ return false;
+ }
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL2, m_settings->m_GD20HM303DGyroHpf, "Failed to set L3GD20H CTRL2");
+}
+
+bool RTIMUGD20HM303D::setGyroCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20HM303DGyroFsr) {
+ case L3GD20H_FSR_245:
+ ctrl4 = 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_500:
+ ctrl4 = 0x10;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_2000:
+ ctrl4 = 0x20;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H FSR code %d\n", m_settings->m_GD20HM303DGyroFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL4, ctrl4, "Failed to set L3GD20H CTRL4");
+}
+
+
+bool RTIMUGD20HM303D::setGyroCTRL5()
+{
+ unsigned char ctrl5;
+
+ // Turn on hpf
+
+ ctrl5 = 0x10;
+
+#ifdef GD20HM303D_CACHE_MODE
+ // turn on fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, ctrl5, "Failed to set L3GD20H CTRL5");
+}
+
+
+bool RTIMUGD20HM303D::setAccelCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_GD20HM303DAccelSampleRate < 0) || (m_settings->m_GD20HM303DAccelSampleRate > 10)) {
+ HAL_ERROR1("Illegal LSM303D accel sample rate code %d\n", m_settings->m_GD20HM303DAccelSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_GD20HM303DAccelSampleRate << 4) | 0x07;
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL1, ctrl1, "Failed to set LSM303D CTRL1");
+}
+
+bool RTIMUGD20HM303D::setAccelCTRL2()
+{
+ unsigned char ctrl2;
+
+ if ((m_settings->m_GD20HM303DAccelLpf < 0) || (m_settings->m_GD20HM303DAccelLpf > 3)) {
+ HAL_ERROR1("Illegal LSM303D accel low pass fiter code %d\n", m_settings->m_GD20HM303DAccelLpf);
+ return false;
+ }
+
+ switch (m_settings->m_GD20HM303DAccelFsr) {
+ case LSM303D_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.000061;
+ break;
+
+ case LSM303D_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.000122;
+ break;
+
+ case LSM303D_ACCEL_FSR_6:
+ m_accelScale = (RTFLOAT)0.000183;
+ break;
+
+ case LSM303D_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.000244;
+ break;
+
+ case LSM303D_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.000732;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303D accel FSR code %d\n", m_settings->m_GD20HM303DAccelFsr);
+ return false;
+ }
+
+ ctrl2 = (m_settings->m_GD20HM303DAccelLpf << 6) | (m_settings->m_GD20HM303DAccelFsr << 3);
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL2, ctrl2, "Failed to set LSM303D CTRL2");
+}
+
+
+bool RTIMUGD20HM303D::setCompassCTRL5()
+{
+ unsigned char ctrl5;
+
+ if ((m_settings->m_GD20HM303DCompassSampleRate < 0) || (m_settings->m_GD20HM303DCompassSampleRate > 5)) {
+ HAL_ERROR1("Illegal LSM303D compass sample rate code %d\n", m_settings->m_GD20HM303DCompassSampleRate);
+ return false;
+ }
+
+ ctrl5 = (m_settings->m_GD20HM303DCompassSampleRate << 2);
+
+#ifdef GD20HM303D_CACHE_MODE
+ // enable fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL5, ctrl5, "Failed to set LSM303D CTRL5");
+}
+
+bool RTIMUGD20HM303D::setCompassCTRL6()
+{
+ unsigned char ctrl6;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_GD20HM303DCompassFsr) {
+ case LSM303D_COMPASS_FSR_2:
+ ctrl6 = 0;
+ m_compassScale = (RTFLOAT)0.008;
+ break;
+
+ case LSM303D_COMPASS_FSR_4:
+ ctrl6 = 0x20;
+ m_compassScale = (RTFLOAT)0.016;
+ break;
+
+ case LSM303D_COMPASS_FSR_8:
+ ctrl6 = 0x40;
+ m_compassScale = (RTFLOAT)0.032;
+ break;
+
+ case LSM303D_COMPASS_FSR_12:
+ ctrl6 = 0x60;
+ m_compassScale = (RTFLOAT)0.0479;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303D compass FSR code %d\n", m_settings->m_GD20HM303DCompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL6, ctrl6, "Failed to set LSM303D CTRL6");
+}
+
+bool RTIMUGD20HM303D::setCompassCTRL7()
+{
+ return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM303D_CTRL7, 0x60, "Failed to set LSM303D CTRL7");
+}
+
+
+int RTIMUGD20HM303D::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20HM303D::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+
+#ifdef GD20HM303D_CACHE_MODE
+ int count;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_FIFO_SRC, 1, &status, "Failed to read L3GD20H fifo status"))
+ return false;
+
+ if ((status & 0x40) != 0) {
+ HAL_INFO("L3GD20H fifo overrun\n");
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x10, "Failed to set L3GD20H CTRL5"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x0, "Failed to set L3GD20H FIFO mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20H FIFO mode"))
+ return false;
+
+ if (!setGyroCTRL5())
+ return false;
+
+ m_imuData.timestamp += m_sampleInterval * 32;
+ return false;
+ }
+
+ // get count of samples in fifo
+ count = status & 0x1f;
+
+ if ((m_cacheCount == 0) && (count > 0) && (count < GD20HM303D_FIFO_THRESH)) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6, accelData, "Failed to read LSM303D accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6, compassData, "Failed to read LSM303D compass data"))
+ return false;
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ } else {
+ if (count >= GD20HM303D_FIFO_THRESH) {
+ // need to create a cache block
+
+ if (m_cacheCount == GD20HM303D_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == GD20HM303D_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, GD20HM303D_FIFO_CHUNK_SIZE * GD20HM303D_FIFO_THRESH,
+ m_cache[m_cacheIn].data, "Failed to read L3GD20H fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6,
+ m_cache[m_cacheIn].accel, "Failed to read LSM303D accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6,
+ m_cache[m_cacheIn].compass, "Failed to read LSM303D compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = GD20HM303D_FIFO_THRESH;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == GD20HM303D_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20HM303D_FIFO_CHUNK_SIZE);
+ memcpy(accelData, m_cache[m_cacheOut].accel, 6);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 6);
+
+ m_cache[m_cacheOut].index += GD20HM303D_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == GD20HM303D_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ }
+
+#else
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status, "Failed to read L3GD20H status"))
+ return false;
+
+ if ((status & 0x8) == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6, accelData, "Failed to read LSM303D accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6, compassData, "Failed to read LSM303D compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+ RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out compass axes
+
+ m_imuData.compass.setY(-m_imuData.compass.y());
+ m_imuData.compass.setZ(-m_imuData.compass.z());
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h
new file mode 100644
index 0000000..7c182f5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303D.h
@@ -0,0 +1,95 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of
+// this software and associated documentation files (the "Software"), to deal in
+// the Software without restriction, including without limitation the rights to use,
+// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished to do so,
+// subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+#ifndef _RTIMUGD20HM303D_H
+#define _RTIMUGD20HM303D_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define GD20HM303D_CACHE_MODE // not reliable at the moment
+
+#ifdef GD20HM303D_CACHE_MODE
+
+// Cache defs
+
+#define GD20HM303D_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define GD20HM303D_FIFO_THRESH 16 // threshold point in fifo
+#define GD20HM303D_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[GD20HM303D_FIFO_THRESH * GD20HM303D_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} GD20HM303D_CACHE_BLOCK;
+
+#endif
+
+class RTIMUGD20HM303D : public RTIMU
+{
+public:
+ RTIMUGD20HM303D(RTIMUSettings *settings);
+ ~RTIMUGD20HM303D();
+
+ virtual const char *IMUName() { return "L3GD20H + LSM303D"; }
+ virtual int IMUType() { return RTIMU_TYPE_GD20HM303D; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL2();
+ bool setGyroCTRL4();
+ bool setGyroCTRL5();
+ bool setAccelCTRL1();
+ bool setAccelCTRL2();
+ bool setCompassCTRL5();
+ bool setCompassCTRL6();
+ bool setCompassCTRL7();
+
+ unsigned char m_gyroSlaveAddr; // I2C address of L3GD20H
+ unsigned char m_accelCompassSlaveAddr; // I2C address of LSM303D
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScale;
+
+#ifdef GD20HM303D_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ GD20HM303D_CACHE_BLOCK m_cache[GD20HM303D_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMUGD20HM303D_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp
new file mode 100644
index 0000000..aeb4be6
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.cpp
@@ -0,0 +1,562 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of
+// this software and associated documentation files (the "Software"), to deal in
+// the Software without restriction, including without limitation the rights to use,
+// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished to do so,
+// subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+#include "RTIMUGD20HM303DLHC.h"
+#include "RTIMUSettings.h"
+
+// this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA 0.2f
+
+RTIMUGD20HM303DLHC::RTIMUGD20HM303DLHC(RTIMUSettings *settings) : RTIMU(settings)
+{
+ m_sampleRate = 100;
+}
+
+RTIMUGD20HM303DLHC::~RTIMUGD20HM303DLHC()
+{
+}
+
+bool RTIMUGD20HM303DLHC::IMUInit()
+{
+ unsigned char result;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+ m_firstTime = true;
+ m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+ // set validity flags
+
+ m_imuData.fusionPoseValid = false;
+ m_imuData.fusionQPoseValid = false;
+ m_imuData.gyroValid = true;
+ m_imuData.accelValid = true;
+ m_imuData.compassValid = true;
+ m_imuData.pressureValid = false;
+ m_imuData.temperatureValid = false;
+ m_imuData.humidityValid = false;
+
+ // configure IMU
+
+ m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+ m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
+ m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
+
+ setCalibrationData();
+
+ // enable the I2C bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // Set up the gyro
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H"))
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id"))
+ return false;
+
+ if (result != L3GD20H_ID) {
+ HAL_ERROR1("Incorrect L3GD20H id %d\n", result);
+ return false;
+ }
+
+ if (!setGyroSampleRate())
+ return false;
+
+ if (!setGyroCTRL2())
+ return false;
+
+ if (!setGyroCTRL4())
+ return false;
+
+ // Set up the accel
+
+ if (!setAccelCTRL1())
+ return false;
+
+ if (!setAccelCTRL4())
+ return false;
+
+ // Set up the compass
+
+ if (!setCompassCRA())
+ return false;
+
+ if (!setCompassCRB())
+ return false;
+
+ if (!setCompassCRM())
+ return false;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+
+ // turn on gyro fifo
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
+ return false;
+#endif
+
+ if (!setGyroCTRL5())
+ return false;
+
+ gyroBiasInit();
+
+ HAL_INFO("GD20HM303DLHC init complete\n");
+ return true;
+}
+
+bool RTIMUGD20HM303DLHC::setGyroSampleRate()
+{
+ unsigned char ctrl1;
+ unsigned char lowOdr = 0;
+
+ switch (m_settings->m_GD20HM303DLHCGyroSampleRate) {
+ case L3GD20H_SAMPLERATE_12_5:
+ ctrl1 = 0x0f;
+ lowOdr = 1;
+ m_sampleRate = 13;
+ break;
+
+ case L3GD20H_SAMPLERATE_25:
+ ctrl1 = 0x4f;
+ lowOdr = 1;
+ m_sampleRate = 25;
+ break;
+
+ case L3GD20H_SAMPLERATE_50:
+ ctrl1 = 0x8f;
+ lowOdr = 1;
+ m_sampleRate = 50;
+ break;
+
+ case L3GD20H_SAMPLERATE_100:
+ ctrl1 = 0x0f;
+ m_sampleRate = 100;
+ break;
+
+ case L3GD20H_SAMPLERATE_200:
+ ctrl1 = 0x4f;
+ m_sampleRate = 200;
+ break;
+
+ case L3GD20H_SAMPLERATE_400:
+ ctrl1 = 0x8f;
+ m_sampleRate = 400;
+ break;
+
+ case L3GD20H_SAMPLERATE_800:
+ ctrl1 = 0xcf;
+ m_sampleRate = 800;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H sample rate code %d\n", m_settings->m_GD20HM303DLHCGyroSampleRate);
+ return false;
+ }
+
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+ switch (m_settings->m_GD20HM303DLHCGyroBW) {
+ case L3GD20H_BANDWIDTH_0:
+ ctrl1 |= 0x00;
+ break;
+
+ case L3GD20H_BANDWIDTH_1:
+ ctrl1 |= 0x10;
+ break;
+
+ case L3GD20H_BANDWIDTH_2:
+ ctrl1 |= 0x20;
+ break;
+
+ case L3GD20H_BANDWIDTH_3:
+ ctrl1 |= 0x30;
+ break;
+
+ }
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, lowOdr, "Failed to set L3GD20H LOW_ODR"))
+ return false;
+
+ return (m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL1, ctrl1, "Failed to set L3GD20H CTRL1"));
+}
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL2()
+{
+ if ((m_settings->m_GD20HM303DLHCGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DLHCGyroHpf > L3GD20H_HPF_9)) {
+ HAL_ERROR1("Illegal L3GD20H high pass filter code %d\n", m_settings->m_GD20HM303DLHCGyroHpf);
+ return false;
+ }
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL2, m_settings->m_GD20HM303DLHCGyroHpf, "Failed to set L3GD20H CTRL2");
+}
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20HM303DLHCGyroFsr) {
+ case L3GD20H_FSR_245:
+ ctrl4 = 0x00;
+ m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_500:
+ ctrl4 = 0x10;
+ m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ case L3GD20H_FSR_2000:
+ ctrl4 = 0x20;
+ m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal L3GD20H FSR code %d\n", m_settings->m_GD20HM303DLHCGyroFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL4, ctrl4, "Failed to set L3GD20H CTRL4");
+}
+
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL5()
+{
+ unsigned char ctrl5;
+
+ // Turn on hpf
+
+ ctrl5 = 0x10;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+ // turn on fifo
+
+ ctrl5 |= 0x40;
+#endif
+
+ return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, ctrl5, "Failed to set L3GD20H CTRL5");
+}
+
+
+bool RTIMUGD20HM303DLHC::setAccelCTRL1()
+{
+ unsigned char ctrl1;
+
+ if ((m_settings->m_GD20HM303DLHCAccelSampleRate < 1) || (m_settings->m_GD20HM303DLHCAccelSampleRate > 7)) {
+ HAL_ERROR1("Illegal LSM303DLHC accel sample rate code %d\n", m_settings->m_GD20HM303DLHCAccelSampleRate);
+ return false;
+ }
+
+ ctrl1 = (m_settings->m_GD20HM303DLHCAccelSampleRate << 4) | 0x07;
+
+ return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL1_A, ctrl1, "Failed to set LSM303D CTRL1");
+}
+
+bool RTIMUGD20HM303DLHC::setAccelCTRL4()
+{
+ unsigned char ctrl4;
+
+ switch (m_settings->m_GD20HM303DLHCAccelFsr) {
+ case LSM303DLHC_ACCEL_FSR_2:
+ m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_4:
+ m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_8:
+ m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)16;
+ break;
+
+ case LSM303DLHC_ACCEL_FSR_16:
+ m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)16;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303DLHC accel FSR code %d\n", m_settings->m_GD20HM303DLHCAccelFsr);
+ return false;
+ }
+
+ ctrl4 = 0x80 + (m_settings->m_GD20HM303DLHCAccelFsr << 4);
+
+ return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL4_A, ctrl4, "Failed to set LSM303DLHC CTRL4");
+}
+
+
+bool RTIMUGD20HM303DLHC::setCompassCRA()
+{
+ unsigned char cra;
+
+ if ((m_settings->m_GD20HM303DLHCCompassSampleRate < 0) || (m_settings->m_GD20HM303DLHCCompassSampleRate > 7)) {
+ HAL_ERROR1("Illegal LSM303DLHC compass sample rate code %d\n", m_settings->m_GD20HM303DLHCCompassSampleRate);
+ return false;
+ }
+
+ cra = (m_settings->m_GD20HM303DLHCCompassSampleRate << 2);
+
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRA_M, cra, "Failed to set LSM303DLHC CRA_M");
+}
+
+bool RTIMUGD20HM303DLHC::setCompassCRB()
+{
+ unsigned char crb;
+
+ // convert FSR to uT
+
+ switch (m_settings->m_GD20HM303DLHCCompassFsr) {
+ case LSM303DLHC_COMPASS_FSR_1_3:
+ crb = 0x20;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)1100;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)980;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_1_9:
+ crb = 0x40;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)855;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)760;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_2_5:
+ crb = 0x60;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)670;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)600;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_4:
+ crb = 0x80;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)450;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)400;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_4_7:
+ crb = 0xa0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)400;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)355;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_5_6:
+ crb = 0xc0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)330;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)295;
+ break;
+
+ case LSM303DLHC_COMPASS_FSR_8_1:
+ crb = 0xe0;
+ m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)230;
+ m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)205;
+ break;
+
+ default:
+ HAL_ERROR1("Illegal LSM303DLHC compass FSR code %d\n", m_settings->m_GD20HM303DLHCCompassFsr);
+ return false;
+ }
+
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRB_M, crb, "Failed to set LSM303DLHC CRB_M");
+}
+
+bool RTIMUGD20HM303DLHC::setCompassCRM()
+{
+ return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRM_M, 0x00, "Failed to set LSM303DLHC CRM_M");
+}
+
+
+int RTIMUGD20HM303DLHC::IMUGetPollInterval()
+{
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20HM303DLHC::IMURead()
+{
+ unsigned char status;
+ unsigned char gyroData[6];
+ unsigned char accelData[6];
+ unsigned char compassData[6];
+
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+ int count;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_FIFO_SRC, 1, &status, "Failed to read L3GD20 fifo status"))
+ return false;
+
+ if ((status & 0x40) != 0) {
+ HAL_INFO("L3GD20 fifo overrun\n");
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x10, "Failed to set L3GD20 CTRL5"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x0, "Failed to set L3GD20 FIFO mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
+ return false;
+
+ if (!setGyroCTRL5())
+ return false;
+
+ m_imuData.timestamp += m_sampleInterval * 32;
+ return false;
+ }
+
+ // get count of samples in fifo
+ count = status & 0x1f;
+
+ if ((m_cacheCount == 0) && (count > 0) && (count < GD20HM303DLHC_FIFO_THRESH)) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ } else {
+ if (count >= GD20HM303DLHC_FIFO_THRESH) {
+ // need to create a cache block
+
+ if (m_cacheCount == GD20HM303DLHC_CACHE_BLOCK_COUNT) {
+ // all cache blocks are full - discard oldest and update timestamp to account for lost samples
+ m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
+ if (++m_cacheOut == GD20HM303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, GD20HM303DLHC_FIFO_CHUNK_SIZE * GD20HM303DLHC_FIFO_THRESH,
+ m_cache[m_cacheIn].data, "Failed to read L3GD20 fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6,
+ m_cache[m_cacheIn].accel, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6,
+ m_cache[m_cacheIn].compass, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+ m_cache[m_cacheIn].count = GD20HM303DLHC_FIFO_THRESH;
+ m_cache[m_cacheIn].index = 0;
+
+ m_cacheCount++;
+ if (++m_cacheIn == GD20HM303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheIn = 0;
+
+ }
+
+ // now fifo has been read if necessary, get something to process
+
+ if (m_cacheCount == 0)
+ return false;
+
+ memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20HM303DLHC_FIFO_CHUNK_SIZE);
+ memcpy(accelData, m_cache[m_cacheOut].accel, 6);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 6);
+
+ m_cache[m_cacheOut].index += GD20HM303DLHC_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == GD20HM303DLHC_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ if (m_firstTime)
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+ else
+ m_imuData.timestamp += m_sampleInterval;
+
+ m_firstTime = false;
+ }
+
+#else
+ if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status, "Failed to read L3GD20H status"))
+ return false;
+
+ if ((status & 0x8) == 0)
+ return false;
+
+ if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
+ return false;
+
+ m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
+
+ if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
+ return false;
+
+ if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
+ return false;
+
+#endif
+
+ RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
+ RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
+
+ m_imuData.compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY);
+ m_imuData.compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY);
+ m_imuData.compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ);
+
+ // sort out gyro axes
+
+ m_imuData.gyro.setX(m_imuData.gyro.x());
+ m_imuData.gyro.setY(-m_imuData.gyro.y());
+ m_imuData.gyro.setZ(-m_imuData.gyro.z());
+
+ // sort out accel data;
+
+ m_imuData.accel.setX(-m_imuData.accel.x());
+
+ // sort out compass axes
+
+ RTFLOAT temp;
+
+ temp = m_imuData.compass.z();
+ m_imuData.compass.setZ(-m_imuData.compass.y());
+ m_imuData.compass.setY(-temp);
+
+ // now do standard processing
+
+ handleGyroBias();
+ calibrateAverageCompass();
+ calibrateAccel();
+
+ // now update the filter
+
+ updateFusion();
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h
new file mode 100644
index 0000000..88876c5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20HM303DLHC.h
@@ -0,0 +1,98 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// This file is part of RTIMULib
+//
+// Copyright (c) 2014-2015, richards-tech, LLC
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy of
+// this software and associated documentation files (the "Software"), to deal in
+// the Software without restriction, including without limitation the rights to use,
+// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished to do so,
+// subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+#ifndef _RTIMUGD20HM303DLHC_H
+#define _RTIMUGD20HM303DLHC_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+//#define GD20M303DLHC_CACHE_MODE // not reliable at the moment
+
+
+#ifdef GD20M303DLHC_CACHE_MODE
+
+// Cache defs
+
+#define GD20M303DLHC_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data
+#define GD20M303DLHC_FIFO_THRESH 16 // threshold point in fifo
+#define GD20M303DLHC_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[GD20M303DLHC_FIFO_THRESH * GD20M303DLHC_FIFO_CHUNK_SIZE];
+ int count; // number of chunks in the cache block
+ int index; // current index into the cache
+ unsigned char accel[6]; // the raw accel readings for the block
+ unsigned char compass[6]; // the raw compass readings for the block
+
+} GD20M303DLHC_CACHE_BLOCK;
+
+#endif
+
+class RTIMUGD20HM303DLHC : public RTIMU
+{
+public:
+ RTIMUGD20HM303DLHC(RTIMUSettings *settings);
+ ~RTIMUGD20HM303DLHC();
+
+ virtual const char *IMUName() { return "L3GD20H + LSM303DLHC"; }
+ virtual int IMUType() { return RTIMU_TYPE_GD20HM303DLHC; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+
+private:
+ bool setGyroSampleRate();
+ bool setGyroCTRL2();
+ bool setGyroCTRL4();
+ bool setGyroCTRL5();
+ bool setAccelCTRL1();
+ bool setAccelCTRL4();
+ bool setCompassCRA();
+ bool setCompassCRB();
+ bool setCompassCRM();
+
+ unsigned char m_gyroSlaveAddr; // I2C address of L3GD20
+ unsigned char m_accelSlaveAddr; // I2C address of LSM303DLHC accel
+ unsigned char m_compassSlaveAddr; // I2C address of LSM303DLHC compass
+
+ RTFLOAT m_gyroScale;
+ RTFLOAT m_accelScale;
+ RTFLOAT m_compassScaleXY;
+ RTFLOAT m_compassScaleZ;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+ bool m_firstTime; // if first sample
+
+ GD20M303DLHC_CACHE_BLOCK m_cache[GD20M303DLHC_CACHE_BLOCK_COUNT]; // the cache itself
+ int m_cacheIn; // the in index
+ int m_cacheOut; // the out index
+ int m_cacheCount; // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMUGD20HM303DLHC_H