http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp new file mode 100644 index 0000000..d8ab502 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.cpp @@ -0,0 +1,537 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 "RTIMUGD20M303DLHC.h" +#include "RTIMUSettings.h" + +// this sets the learning rate for compass running average calculation + +#define COMPASS_ALPHA 0.2f + +RTIMUGD20M303DLHC::RTIMUGD20M303DLHC(RTIMUSettings *settings) : RTIMU(settings) +{ + m_sampleRate = 100; +} + +RTIMUGD20M303DLHC::~RTIMUGD20M303DLHC() +{ +} + +bool RTIMUGD20M303DLHC::IMUInit() +{ + unsigned char result; + +#ifdef GD20M303DLHC_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, L3GD20_CTRL5, 0x80, "Failed to boot L3GD20")) + return false; + + if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_WHO_AM_I, 1, &result, "Failed to read L3GD20 id")) + return false; + + if (result != L3GD20_ID) { + HAL_ERROR1("Incorrect L3GD20 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 GD20M303DLHC_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("GD20M303DLHC init complete\n"); + return true; +} + +bool RTIMUGD20M303DLHC::setGyroSampleRate() +{ + unsigned char ctrl1; + + switch (m_settings->m_GD20M303DLHCGyroSampleRate) { + case L3GD20_SAMPLERATE_95: + ctrl1 = 0x0f; + m_sampleRate = 95; + break; + + case L3GD20_SAMPLERATE_190: + ctrl1 = 0x4f; + m_sampleRate = 190; + break; + + case L3GD20_SAMPLERATE_380: + ctrl1 = 0x8f; + m_sampleRate = 380; + break; + + case L3GD20_SAMPLERATE_760: + ctrl1 = 0xcf; + m_sampleRate = 760; + break; + + default: + HAL_ERROR1("Illegal L3GD20 sample rate code %d\n", m_settings->m_GD20M303DLHCGyroSampleRate); + return false; + } + + m_sampleInterval = (uint64_t)1000000 / m_sampleRate; + + switch (m_settings->m_GD20M303DLHCGyroBW) { + case L3GD20_BANDWIDTH_0: + ctrl1 |= 0x00; + break; + + case L3GD20_BANDWIDTH_1: + ctrl1 |= 0x10; + break; + + case L3GD20_BANDWIDTH_2: + ctrl1 |= 0x20; + break; + + case L3GD20_BANDWIDTH_3: + ctrl1 |= 0x30; + break; + + } + + return (m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL1, ctrl1, "Failed to set L3GD20 CTRL1")); +} + +bool RTIMUGD20M303DLHC::setGyroCTRL2() +{ + if ((m_settings->m_GD20M303DLHCGyroHpf < L3GD20_HPF_0) || (m_settings->m_GD20M303DLHCGyroHpf > L3GD20_HPF_9)) { + HAL_ERROR1("Illegal L3GD20 high pass filter code %d\n", m_settings->m_GD20M303DLHCGyroHpf); + return false; + } + return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL2, m_settings->m_GD20M303DLHCGyroHpf, "Failed to set L3GD20 CTRL2"); +} + +bool RTIMUGD20M303DLHC::setGyroCTRL4() +{ + unsigned char ctrl4; + + switch (m_settings->m_GD20M303DLHCGyroFsr) { + case L3GD20_FSR_250: + ctrl4 = 0x00; + m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD; + break; + + case L3GD20_FSR_500: + ctrl4 = 0x10; + m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD; + break; + + case L3GD20_FSR_2000: + ctrl4 = 0x20; + m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD; + break; + + default: + HAL_ERROR1("Illegal L3GD20 FSR code %d\n", m_settings->m_GD20M303DLHCGyroFsr); + return false; + } + + return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL4, ctrl4, "Failed to set L3GD20 CTRL4"); +} + + +bool RTIMUGD20M303DLHC::setGyroCTRL5() +{ + unsigned char ctrl5; + + // Turn on hpf + + ctrl5 = 0x10; + +#ifdef GD20M303DLHC_CACHE_MODE + // turn on fifo + + ctrl5 |= 0x40; +#endif + + return m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL5, ctrl5, "Failed to set L3GD20 CTRL5"); +} + + +bool RTIMUGD20M303DLHC::setAccelCTRL1() +{ + unsigned char ctrl1; + + if ((m_settings->m_GD20M303DLHCAccelSampleRate < 1) || (m_settings->m_GD20M303DLHCAccelSampleRate > 7)) { + HAL_ERROR1("Illegal LSM303DLHC accel sample rate code %d\n", m_settings->m_GD20M303DLHCAccelSampleRate); + return false; + } + + ctrl1 = (m_settings->m_GD20M303DLHCAccelSampleRate << 4) | 0x07; + + return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL1_A, ctrl1, "Failed to set LSM303D CTRL1"); +} + +bool RTIMUGD20M303DLHC::setAccelCTRL4() +{ + unsigned char ctrl4; + + switch (m_settings->m_GD20M303DLHCAccelFsr) { + 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_GD20M303DLHCAccelFsr); + return false; + } + + ctrl4 = 0x80 + (m_settings->m_GD20M303DLHCAccelFsr << 4); + + return m_settings->HALWrite(m_accelSlaveAddr, LSM303DLHC_CTRL4_A, ctrl4, "Failed to set LSM303DLHC CTRL4"); +} + + +bool RTIMUGD20M303DLHC::setCompassCRA() +{ + unsigned char cra; + + if ((m_settings->m_GD20M303DLHCCompassSampleRate < 0) || (m_settings->m_GD20M303DLHCCompassSampleRate > 7)) { + HAL_ERROR1("Illegal LSM303DLHC compass sample rate code %d\n", m_settings->m_GD20M303DLHCCompassSampleRate); + return false; + } + + cra = (m_settings->m_GD20M303DLHCCompassSampleRate << 2); + + return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRA_M, cra, "Failed to set LSM303DLHC CRA_M"); +} + +bool RTIMUGD20M303DLHC::setCompassCRB() +{ + unsigned char crb; + + // convert FSR to uT + + switch (m_settings->m_GD20M303DLHCCompassFsr) { + 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_GD20M303DLHCCompassFsr); + return false; + } + + return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRB_M, crb, "Failed to set LSM303DLHC CRB_M"); +} + +bool RTIMUGD20M303DLHC::setCompassCRM() +{ + return m_settings->HALWrite(m_compassSlaveAddr, LSM303DLHC_CRM_M, 0x00, "Failed to set LSM303DLHC CRM_M"); +} + + +int RTIMUGD20M303DLHC::IMUGetPollInterval() +{ + return (400 / m_sampleRate); +} + +bool RTIMUGD20M303DLHC::IMURead() +{ + unsigned char status; + unsigned char gyroData[6]; + unsigned char accelData[6]; + unsigned char compassData[6]; + + +#ifdef GD20M303DLHC_CACHE_MODE + int count; + + if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_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, L3GD20_CTRL5, 0x10, "Failed to set L3GD20 CTRL5")) + return false; + + if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x0, "Failed to set L3GD20 FIFO mode")) + return false; + + if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_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 < GD20M303DLHC_FIFO_THRESH)) { + // special case of a small fifo and nothing cached - just handle as simple read + + if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_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 >= GD20M303DLHC_FIFO_THRESH) { + // need to create a cache block + + if (m_cacheCount == GD20M303DLHC_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 == GD20M303DLHC_CACHE_BLOCK_COUNT) + m_cacheOut = 0; + m_cacheCount--; + } + + if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, GD20M303DLHC_FIFO_CHUNK_SIZE * GD20M303DLHC_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 = GD20M303DLHC_FIFO_THRESH; + m_cache[m_cacheIn].index = 0; + + m_cacheCount++; + if (++m_cacheIn == GD20M303DLHC_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, GD20M303DLHC_FIFO_CHUNK_SIZE); + memcpy(accelData, m_cache[m_cacheOut].accel, 6); + memcpy(compassData, m_cache[m_cacheOut].compass, 6); + + m_cache[m_cacheOut].index += GD20M303DLHC_FIFO_CHUNK_SIZE; + + if (--m_cache[m_cacheOut].count == 0) { + // this cache block is now empty + + if (++m_cacheOut == GD20M303DLHC_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, L3GD20_STATUS, 1, &status, "Failed to read L3GD20 status")) + return false; + + if ((status & 0x8) == 0) + return false; + + if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, 6, gyroData, "Failed to read L3GD20 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/RTIMUGD20M303DLHC.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h new file mode 100644 index 0000000..6d2aa9c --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUGD20M303DLHC.h @@ -0,0 +1,97 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 _RTIMUGD20M303DLHC_H +#define _RTIMUGD20M303DLHC_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 RTIMUGD20M303DLHC : public RTIMU +{ +public: + RTIMUGD20M303DLHC(RTIMUSettings *settings); + ~RTIMUGD20M303DLHC(); + + virtual const char *IMUName() { return "L3GD20 + LSM303DLHC"; } + virtual int IMUType() { return RTIMU_TYPE_GD20M303DLHC; } + 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 // _RTIMUGD20M303DLHC_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp new file mode 100644 index 0000000..676d111 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.cpp @@ -0,0 +1,538 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 "RTIMULSM9DS0.h" +#include "RTIMUSettings.h" + +// this sets the learning rate for compass running average calculation + +#define COMPASS_ALPHA 0.2f + +RTIMULSM9DS0::RTIMULSM9DS0(RTIMUSettings *settings) : RTIMU(settings) +{ + m_sampleRate = 100; +} + +RTIMULSM9DS0::~RTIMULSM9DS0() +{ +} + +bool RTIMULSM9DS0::IMUInit() +{ + unsigned char result; + +#ifdef LSM9DS0_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 accelmag address + + if (m_settings->HALRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &result, "")) { + if (result == LSM9DS0_ACCELMAG_ID) { + m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0; + } + } else { + m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1; + } + + setCalibrationData(); + + // enable the I2C bus + + if (!m_settings->HALOpen()) + return false; + + // Set up the gyro + + if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0")) + return false; + + if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 gyro id")) + return false; + + if (result != LSM9DS0_GYRO_ID) { + HAL_ERROR1("Incorrect LSM9DS0 gyro id %d\n", result); + return false; + } + + if (!setGyroSampleRate()) + return false; + + if (!setGyroCTRL2()) + return false; + + if (!setGyroCTRL4()) + return false; + + // Set up the accel + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id")) + return false; + + if (result != LSM9DS0_ACCELMAG_ID) { + HAL_ERROR1("Incorrect LSM9DS0 accel/mag 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 LSM9DS0_CACHE_MODE + + // turn on gyro fifo + + if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode")) + return false; +#endif + + if (!setGyroCTRL5()) + return false; + + gyroBiasInit(); + + HAL_INFO("LSM9DS0 init complete\n"); + return true; +} + +bool RTIMULSM9DS0::setGyroSampleRate() +{ + unsigned char ctrl1; + + switch (m_settings->m_LSM9DS0GyroSampleRate) { + case LSM9DS0_GYRO_SAMPLERATE_95: + ctrl1 = 0x0f; + m_sampleRate = 95; + break; + + case LSM9DS0_GYRO_SAMPLERATE_190: + ctrl1 = 0x4f; + m_sampleRate = 190; + break; + + case LSM9DS0_GYRO_SAMPLERATE_380: + ctrl1 = 0x8f; + m_sampleRate = 380; + break; + + case LSM9DS0_GYRO_SAMPLERATE_760: + ctrl1 = 0xcf; + m_sampleRate = 760; + break; + + default: + HAL_ERROR1("Illegal LSM9DS0 gyro sample rate code %d\n", m_settings->m_LSM9DS0GyroSampleRate); + return false; + } + + m_sampleInterval = (uint64_t)1000000 / m_sampleRate; + + switch (m_settings->m_LSM9DS0GyroBW) { + case LSM9DS0_GYRO_BANDWIDTH_0: + ctrl1 |= 0x00; + break; + + case LSM9DS0_GYRO_BANDWIDTH_1: + ctrl1 |= 0x10; + break; + + case LSM9DS0_GYRO_BANDWIDTH_2: + ctrl1 |= 0x20; + break; + + case LSM9DS0_GYRO_BANDWIDTH_3: + ctrl1 |= 0x30; + break; + + } + + return (m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL1, ctrl1, "Failed to set LSM9DS0 gyro CTRL1")); +} + +bool RTIMULSM9DS0::setGyroCTRL2() +{ + if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) { + HAL_ERROR1("Illegal LSM9DS0 gyro high pass filter code %d\n", m_settings->m_LSM9DS0GyroHpf); + return false; + } + return m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf, "Failed to set LSM9DS0 gyro CTRL2"); +} + +bool RTIMULSM9DS0::setGyroCTRL4() +{ + unsigned char ctrl4; + + switch (m_settings->m_LSM9DS0GyroFsr) { + case LSM9DS0_GYRO_FSR_250: + ctrl4 = 0x00; + m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD; + break; + + case LSM9DS0_GYRO_FSR_500: + ctrl4 = 0x10; + m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD; + break; + + case LSM9DS0_GYRO_FSR_2000: + ctrl4 = 0x20; + m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD; + break; + + default: + HAL_ERROR1("Illegal LSM9DS0 gyro FSR code %d\n", m_settings->m_LSM9DS0GyroFsr); + return false; + } + + return m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL4, ctrl4, "Failed to set LSM9DS0 gyro CTRL4"); +} + + +bool RTIMULSM9DS0::setGyroCTRL5() +{ + unsigned char ctrl5; + + // Turn on hpf + + ctrl5 = 0x10; + +#ifdef LSM9DS0_CACHE_MODE + // turn on fifo + + ctrl5 |= 0x40; +#endif + + return m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, ctrl5, "Failed to set LSM9DS0 gyro CTRL5"); +} + + +bool RTIMULSM9DS0::setAccelCTRL1() +{ + unsigned char ctrl1; + + if ((m_settings->m_LSM9DS0AccelSampleRate < 0) || (m_settings->m_LSM9DS0AccelSampleRate > 10)) { + HAL_ERROR1("Illegal LSM9DS0 accel sample rate code %d\n", m_settings->m_LSM9DS0AccelSampleRate); + return false; + } + + ctrl1 = (m_settings->m_LSM9DS0AccelSampleRate << 4) | 0x07; + + return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL1, ctrl1, "Failed to set LSM9DS0 accell CTRL1"); +} + +bool RTIMULSM9DS0::setAccelCTRL2() +{ + unsigned char ctrl2; + + if ((m_settings->m_LSM9DS0AccelLpf < 0) || (m_settings->m_LSM9DS0AccelLpf > 3)) { + HAL_ERROR1("Illegal LSM9DS0 accel low pass fiter code %d\n", m_settings->m_LSM9DS0AccelLpf); + return false; + } + + switch (m_settings->m_LSM9DS0AccelFsr) { + case LSM9DS0_ACCEL_FSR_2: + m_accelScale = (RTFLOAT)0.000061; + break; + + case LSM9DS0_ACCEL_FSR_4: + m_accelScale = (RTFLOAT)0.000122; + break; + + case LSM9DS0_ACCEL_FSR_6: + m_accelScale = (RTFLOAT)0.000183; + break; + + case LSM9DS0_ACCEL_FSR_8: + m_accelScale = (RTFLOAT)0.000244; + break; + + case LSM9DS0_ACCEL_FSR_16: + m_accelScale = (RTFLOAT)0.000732; + break; + + default: + HAL_ERROR1("Illegal LSM9DS0 accel FSR code %d\n", m_settings->m_LSM9DS0AccelFsr); + return false; + } + + ctrl2 = (m_settings->m_LSM9DS0AccelLpf << 6) | (m_settings->m_LSM9DS0AccelFsr << 3); + + return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL2, ctrl2, "Failed to set LSM9DS0 accel CTRL2"); +} + + +bool RTIMULSM9DS0::setCompassCTRL5() +{ + unsigned char ctrl5; + + if ((m_settings->m_LSM9DS0CompassSampleRate < 0) || (m_settings->m_LSM9DS0CompassSampleRate > 5)) { + HAL_ERROR1("Illegal LSM9DS0 compass sample rate code %d\n", m_settings->m_LSM9DS0CompassSampleRate); + return false; + } + + ctrl5 = (m_settings->m_LSM9DS0CompassSampleRate << 2); + +#ifdef LSM9DS0_CACHE_MODE + // enable fifo + + ctrl5 |= 0x40; +#endif + + return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL5, ctrl5, "Failed to set LSM9DS0 compass CTRL5"); +} + +bool RTIMULSM9DS0::setCompassCTRL6() +{ + unsigned char ctrl6; + + // convert FSR to uT + + switch (m_settings->m_LSM9DS0CompassFsr) { + case LSM9DS0_COMPASS_FSR_2: + ctrl6 = 0; + m_compassScale = (RTFLOAT)0.008; + break; + + case LSM9DS0_COMPASS_FSR_4: + ctrl6 = 0x20; + m_compassScale = (RTFLOAT)0.016; + break; + + case LSM9DS0_COMPASS_FSR_8: + ctrl6 = 0x40; + m_compassScale = (RTFLOAT)0.032; + break; + + case LSM9DS0_COMPASS_FSR_12: + ctrl6 = 0x60; + m_compassScale = (RTFLOAT)0.0479; + break; + + default: + HAL_ERROR1("Illegal LSM9DS0 compass FSR code %d\n", m_settings->m_LSM9DS0CompassFsr); + return false; + } + + return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL6, ctrl6, "Failed to set LSM9DS0 compass CTRL6"); +} + +bool RTIMULSM9DS0::setCompassCTRL7() +{ + return m_settings->HALWrite(m_accelCompassSlaveAddr, LSM9DS0_CTRL7, 0x60, "Failed to set LSM9DS0CTRL7"); +} + + + +int RTIMULSM9DS0::IMUGetPollInterval() +{ + return (400 / m_sampleRate); +} + +bool RTIMULSM9DS0::IMURead() +{ + unsigned char status; + unsigned char gyroData[6]; + unsigned char accelData[6]; + unsigned char compassData[6]; + + +#ifdef LSM9DS0_CACHE_MODE + int count; + + if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_SRC, 1, &status, "Failed to read LSM9DS0 gyro fifo status")) + return false; + + if ((status & 0x40) != 0) { + HAL_INFO("LSM9DS0 gyro fifo overrun\n"); + if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x10, "Failed to set LSM9DS0 gyro CTRL5")) + return false; + + if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x0, "Failed to set LSM9DS0 gyro FIFO mode")) + return false; + + if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 gyro 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 < LSM9DS0_FIFO_THRESH)) { + // special case of a small fifo and nothing cached - just handle as simple read + + if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data")) + return false; + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data")) + return false; + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data")) + return false; + + if (m_firstTime) + m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); + else + m_imuData.timestamp += m_sampleInterval; + + m_firstTime = false; + } else { + if (count >= LSM9DS0_FIFO_THRESH) { + // need to create a cache block + + if (m_cacheCount == LSM9DS0_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 == LSM9DS0_CACHE_BLOCK_COUNT) + m_cacheOut = 0; + m_cacheCount--; + } + + if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, LSM9DS0_FIFO_CHUNK_SIZE * LSM9DS0_FIFO_THRESH, + m_cache[m_cacheIn].data, "Failed to read LSM9DS0 fifo data")) + return false; + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, + m_cache[m_cacheIn].accel, "Failed to read LSM9DS0 accel data")) + return false; + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, + m_cache[m_cacheIn].compass, "Failed to read LSM9DS0 compass data")) + return false; + + m_cache[m_cacheIn].count = LSM9DS0_FIFO_THRESH; + m_cache[m_cacheIn].index = 0; + + m_cacheCount++; + if (++m_cacheIn == LSM9DS0_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, LSM9DS0_FIFO_CHUNK_SIZE); + memcpy(accelData, m_cache[m_cacheOut].accel, 6); + memcpy(compassData, m_cache[m_cacheOut].compass, 6); + + m_cache[m_cacheOut].index += LSM9DS0_FIFO_CHUNK_SIZE; + + if (--m_cache[m_cacheOut].count == 0) { + // this cache block is now empty + + if (++m_cacheOut == LSM9DS0_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, LSM9DS0_GYRO_STATUS, 1, &status, "Failed to read LSM9DS0 status")) + return false; + + if ((status & 0x8) == 0) + return false; + + if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data")) + return false; + + m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data")) + return false; + + if (!m_settings->HALRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 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 and correct for bias + + 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()); + + // 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/RTIMULSM9DS0.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.h new file mode 100644 index 0000000..814bd3f --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS0.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 _RTIMULSM9DS0_H +#define _RTIMULSM9DS0_H + +#include "RTIMU.h" + +// Define this symbol to use cache mode + +//#define LSM9DS0_CACHE_MODE // not reliable at the moment + +#ifdef LSM9DS0_CACHE_MODE + +// Cache defs + +#define LSM9DS0_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data +#define LSM9DS0_FIFO_THRESH 16 // threshold point in fifo +#define LSM9DS0_CACHE_BLOCK_COUNT 16 // number of cache blocks + +typedef struct +{ + unsigned char data[LSM9DS0_FIFO_THRESH * LSM9DS0_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 + +} LSM9DS0_CACHE_BLOCK; + +#endif + +class RTIMULSM9DS0 : public RTIMU +{ +public: + RTIMULSM9DS0(RTIMUSettings *settings); + ~RTIMULSM9DS0(); + + virtual const char *IMUName() { return "LSM9DS0"; } + virtual int IMUType() { return RTIMU_TYPE_LSM9DS0; } + 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 gyro + unsigned char m_accelCompassSlaveAddr; // I2C address of accel and mag + + RTFLOAT m_gyroScale; + RTFLOAT m_accelScale; + RTFLOAT m_compassScale; + +#ifdef LSM9DS0_CACHE_MODE + bool m_firstTime; // if first sample + + LSM9DS0_CACHE_BLOCK m_cache[LSM9DS0_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 // _RTIMULSM9DS0_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp new file mode 100644 index 0000000..b84b6b7 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.cpp @@ -0,0 +1,408 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 "RTIMULSM9DS1.h" +#include "RTIMUSettings.h" + +// this sets the learning rate for compass running average calculation + +#define COMPASS_ALPHA 0.2f + +RTIMULSM9DS1::RTIMULSM9DS1(RTIMUSettings *settings) : RTIMU(settings) +{ + m_sampleRate = 100; +} + +RTIMULSM9DS1::~RTIMULSM9DS1() +{ +} + +bool RTIMULSM9DS1::IMUInit() +{ + unsigned char result; + + // 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_accelGyroSlaveAddr = m_settings->m_I2CSlaveAddress; + + // work outmag address + + if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS0, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { + if (result == LSM9DS1_MAG_ID) { + m_magSlaveAddr = LSM9DS1_MAG_ADDRESS0; + } + } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS1, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { + if (result == LSM9DS1_MAG_ID) { + m_magSlaveAddr = LSM9DS1_MAG_ADDRESS1; + } + } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS2, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { + if (result == LSM9DS1_MAG_ID) { + m_magSlaveAddr = LSM9DS1_MAG_ADDRESS2; + } + } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS3, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { + if (result == LSM9DS1_MAG_ID) { + m_magSlaveAddr = LSM9DS1_MAG_ADDRESS3; + } + } + + setCalibrationData(); + + // enable the I2C bus + + if (!m_settings->HALOpen()) + return false; + + // Set up the gyro/accel + + if (!m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL8, 0x80, "Failed to boot LSM9DS1")) + return false; + + m_settings->delayMs(100); + + if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/gyro id")) + return false; + + if (result != LSM9DS1_ID) { + HAL_ERROR1("Incorrect LSM9DS1 gyro id %d\n", result); + return false; + } + + if (!setGyroSampleRate()) + return false; + + if (!setGyroCTRL3()) + return false; + + // Set up the mag + + if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/mag id")) + return false; + + if (result != LSM9DS1_MAG_ID) { + HAL_ERROR1("Incorrect LSM9DS1 accel/mag id %d\n", result); + return false; + } + + if (!setAccelCTRL6()) + return false; + + if (!setAccelCTRL7()) + return false; + + if (!setCompassCTRL1()) + return false; + + if (!setCompassCTRL2()) + return false; + + if (!setCompassCTRL3()) + return false; + + gyroBiasInit(); + + HAL_INFO("LSM9DS1 init complete\n"); + return true; +} + +bool RTIMULSM9DS1::setGyroSampleRate() +{ + unsigned char ctrl1; + + switch (m_settings->m_LSM9DS1GyroSampleRate) { + case LSM9DS1_GYRO_SAMPLERATE_14_9: + ctrl1 = 0x20; + m_sampleRate = 15; + break; + + case LSM9DS1_GYRO_SAMPLERATE_59_5: + ctrl1 = 0x40; + m_sampleRate = 60; + break; + + case LSM9DS1_GYRO_SAMPLERATE_119: + ctrl1 = 0x60; + m_sampleRate = 119; + break; + + case LSM9DS1_GYRO_SAMPLERATE_238: + ctrl1 = 0x80; + m_sampleRate = 238; + break; + + case LSM9DS1_GYRO_SAMPLERATE_476: + ctrl1 = 0xa0; + m_sampleRate = 476; + break; + + case LSM9DS1_GYRO_SAMPLERATE_952: + ctrl1 = 0xc0; + m_sampleRate = 952; + break; + + default: + HAL_ERROR1("Illegal LSM9DS1 gyro sample rate code %d\n", m_settings->m_LSM9DS1GyroSampleRate); + return false; + } + + m_sampleInterval = (uint64_t)1000000 / m_sampleRate; + + switch (m_settings->m_LSM9DS1GyroBW) { + case LSM9DS1_GYRO_BANDWIDTH_0: + ctrl1 |= 0x00; + break; + + case LSM9DS1_GYRO_BANDWIDTH_1: + ctrl1 |= 0x01; + break; + + case LSM9DS1_GYRO_BANDWIDTH_2: + ctrl1 |= 0x02; + break; + + case LSM9DS1_GYRO_BANDWIDTH_3: + ctrl1 |= 0x03; + break; + } + + switch (m_settings->m_LSM9DS1GyroFsr) { + case LSM9DS1_GYRO_FSR_250: + ctrl1 |= 0x00; + m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD; + break; + + case LSM9DS1_GYRO_FSR_500: + ctrl1 |= 0x08; + m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD; + break; + + case LSM9DS1_GYRO_FSR_2000: + ctrl1 |= 0x18; + m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD; + break; + + default: + HAL_ERROR1("Illegal LSM9DS1 gyro FSR code %d\n", m_settings->m_LSM9DS1GyroFsr); + return false; + } + return (m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL1, ctrl1, "Failed to set LSM9DS1 gyro CTRL1")); +} + +bool RTIMULSM9DS1::setGyroCTRL3() +{ + unsigned char ctrl3; + + if ((m_settings->m_LSM9DS1GyroHpf < LSM9DS1_GYRO_HPF_0) || (m_settings->m_LSM9DS1GyroHpf > LSM9DS1_GYRO_HPF_9)) { + HAL_ERROR1("Illegal LSM9DS1 gyro high pass filter code %d\n", m_settings->m_LSM9DS1GyroHpf); + return false; + } + ctrl3 = m_settings->m_LSM9DS1GyroHpf; + + // Turn on hpf + ctrl3 |= 0x40; + + return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL3, ctrl3, "Failed to set LSM9DS1 gyro CTRL3"); +} + +bool RTIMULSM9DS1::setAccelCTRL6() +{ + unsigned char ctrl6; + + if ((m_settings->m_LSM9DS1AccelSampleRate < 0) || (m_settings->m_LSM9DS1AccelSampleRate > 6)) { + HAL_ERROR1("Illegal LSM9DS1 accel sample rate code %d\n", m_settings->m_LSM9DS1AccelSampleRate); + return false; + } + + ctrl6 = (m_settings->m_LSM9DS1AccelSampleRate << 5); + + if ((m_settings->m_LSM9DS1AccelLpf < 0) || (m_settings->m_LSM9DS1AccelLpf > 3)) { + HAL_ERROR1("Illegal LSM9DS1 accel low pass fiter code %d\n", m_settings->m_LSM9DS1AccelLpf); + return false; + } + + switch (m_settings->m_LSM9DS1AccelFsr) { + case LSM9DS1_ACCEL_FSR_2: + m_accelScale = (RTFLOAT)0.000061; + break; + + case LSM9DS1_ACCEL_FSR_4: + m_accelScale = (RTFLOAT)0.000122; + break; + + case LSM9DS1_ACCEL_FSR_8: + m_accelScale = (RTFLOAT)0.000244; + break; + + case LSM9DS1_ACCEL_FSR_16: + m_accelScale = (RTFLOAT)0.000732; + break; + + default: + HAL_ERROR1("Illegal LSM9DS1 accel FSR code %d\n", m_settings->m_LSM9DS1AccelFsr); + return false; + } + + ctrl6 |= (m_settings->m_LSM9DS1AccelLpf) | (m_settings->m_LSM9DS1AccelFsr << 3); + + return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL6, ctrl6, "Failed to set LSM9DS1 accel CTRL6"); +} + +bool RTIMULSM9DS1::setAccelCTRL7() +{ + unsigned char ctrl7; + + ctrl7 = 0x00; + //Bug: Bad things happen. + //ctrl7 = 0x05; + + return m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL7, ctrl7, "Failed to set LSM9DS1 accel CTRL7"); +} + + +bool RTIMULSM9DS1::setCompassCTRL1() +{ + unsigned char ctrl1; + + if ((m_settings->m_LSM9DS1CompassSampleRate < 0) || (m_settings->m_LSM9DS1CompassSampleRate > 5)) { + HAL_ERROR1("Illegal LSM9DS1 compass sample rate code %d\n", m_settings->m_LSM9DS1CompassSampleRate); + return false; + } + + ctrl1 = (m_settings->m_LSM9DS1CompassSampleRate << 2); + + return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL1, ctrl1, "Failed to set LSM9DS1 compass CTRL5"); +} + +bool RTIMULSM9DS1::setCompassCTRL2() +{ + unsigned char ctrl2; + + // convert FSR to uT + + switch (m_settings->m_LSM9DS1CompassFsr) { + case LSM9DS1_COMPASS_FSR_4: + ctrl2 = 0; + m_compassScale = (RTFLOAT)0.014; + break; + + case LSM9DS1_COMPASS_FSR_8: + ctrl2 = 0x20; + m_compassScale = (RTFLOAT)0.029; + break; + + case LSM9DS1_COMPASS_FSR_12: + ctrl2 = 0x40; + m_compassScale = (RTFLOAT)0.043; + break; + + case LSM9DS1_COMPASS_FSR_16: + ctrl2 = 0x60; + m_compassScale = (RTFLOAT)0.058; + break; + + default: + HAL_ERROR1("Illegal LSM9DS1 compass FSR code %d\n", m_settings->m_LSM9DS1CompassFsr); + return false; + } + + return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL2, ctrl2, "Failed to set LSM9DS1 compass CTRL6"); +} + +bool RTIMULSM9DS1::setCompassCTRL3() +{ + return m_settings->HALWrite(m_magSlaveAddr, LSM9DS1_MAG_CTRL3, 0x00, "Failed to set LSM9DS1 compass CTRL3"); +} + +int RTIMULSM9DS1::IMUGetPollInterval() +{ + return (400 / m_sampleRate); +} + +bool RTIMULSM9DS1::IMURead() +{ + unsigned char status; + unsigned char gyroData[6]; + unsigned char accelData[6]; + unsigned char compassData[6]; + + if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_STATUS, 1, &status, "Failed to read LSM9DS1 status")) + return false; + + if ((status & 0x3) == 0) + return false; + + for (int i = 0; i<6; i++){ + if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_G + i, 1, &gyroData[i], "Failed to read LSM9DS1 gyro data")) + return false; + + if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_XL + i, 1, &accelData[i], "Failed to read LSM9DS1 accel data")) + return false; + + if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_OUT_X_L + i, 1, &compassData[i], "Failed to read LSM9DS1 compass data")) + return false; + } + + + m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); + + 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 and correct for bias + + m_imuData.gyro.setZ(-m_imuData.gyro.z()); + + // sort out accel data; + + m_imuData.accel.setX(-m_imuData.accel.x()); + m_imuData.accel.setY(-m_imuData.accel.y()); + + // sort out compass axes + + m_imuData.compass.setX(-m_imuData.compass.x()); + 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/RTIMULSM9DS1.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h new file mode 100644 index 0000000..7f4feda --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMULSM9DS1.h @@ -0,0 +1,93 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 _RTIMULSM9DS1_H +#define _RTIMULSM9DS1_H + +#include "RTIMU.h" + +// Define this symbol to use cache mode + +//#define LSM9DS1_CACHE_MODE // not reliable at the moment + +#ifdef LSM9DS1_CACHE_MODE + +// Cache defs + +#define LSM9DS1_FIFO_CHUNK_SIZE 6 // 6 bytes of gyro data +#define LSM9DS1_FIFO_THRESH 16 // threshold point in fifo +#define LSM9DS1_CACHE_BLOCK_COUNT 16 // number of cache blocks + +typedef struct +{ + unsigned char data[LSM9DS1_FIFO_THRESH * LSM9DS1_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 + +} LSM9DS1_CACHE_BLOCK; + +#endif + +class RTIMULSM9DS1 : public RTIMU +{ +public: + RTIMULSM9DS1(RTIMUSettings *settings); + ~RTIMULSM9DS1(); + + virtual const char *IMUName() { return "LSM9DS1"; } + virtual int IMUType() { return RTIMU_TYPE_LSM9DS1; } + virtual bool IMUInit(); + virtual int IMUGetPollInterval(); + virtual bool IMURead(); + +private: + bool setGyroSampleRate(); + bool setGyroCTRL3(); + bool setAccelCTRL6(); + bool setAccelCTRL7(); + bool setCompassCTRL1(); + bool setCompassCTRL2(); + bool setCompassCTRL3(); + + unsigned char m_accelGyroSlaveAddr; // I2C address of accel andgyro + unsigned char m_magSlaveAddr; // I2C address of mag + + RTFLOAT m_gyroScale; + RTFLOAT m_accelScale; + RTFLOAT m_compassScale; + +#ifdef LSM9DS1_CACHE_MODE + bool m_firstTime; // if first sample + + LSM9DS1_CACHE_BLOCK m_cache[LSM9DS1_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 // _RTIMULSM9DS1_H http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp new file mode 100644 index 0000000..59d3f31 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.cpp @@ -0,0 +1,633 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 "RTIMUMPU9150.h" +#include "RTIMUSettings.h" + +RTIMUMPU9150::RTIMUMPU9150(RTIMUSettings *settings) : RTIMU(settings) +{ + +} + +RTIMUMPU9150::~RTIMUMPU9150() +{ +} + +bool RTIMUMPU9150::setLpf(unsigned char lpf) +{ + switch (lpf) { + case MPU9150_LPF_256: + case MPU9150_LPF_188: + case MPU9150_LPF_98: + case MPU9150_LPF_42: + case MPU9150_LPF_20: + case MPU9150_LPF_10: + case MPU9150_LPF_5: + m_lpf = lpf; + return true; + + default: + HAL_ERROR1("Illegal MPU9150 lpf %d\n", lpf); + return false; + } +} + + +bool RTIMUMPU9150::setSampleRate(int rate) +{ + if ((rate < MPU9150_SAMPLERATE_MIN) || (rate > MPU9150_SAMPLERATE_MAX)) { + HAL_ERROR1("Illegal sample rate %d\n", rate); + return false; + } + m_sampleRate = rate; + m_sampleInterval = (uint64_t)1000000 / m_sampleRate; + return true; +} + +bool RTIMUMPU9150::setCompassRate(int rate) +{ + if ((rate < MPU9150_COMPASSRATE_MIN) || (rate > MPU9150_COMPASSRATE_MAX)) { + HAL_ERROR1("Illegal compass rate %d\n", rate); + return false; + } + m_compassRate = rate; + return true; +} + +bool RTIMUMPU9150::setGyroFsr(unsigned char fsr) +{ + switch (fsr) { + case MPU9150_GYROFSR_250: + m_gyroFsr = fsr; + m_gyroScale = RTMATH_PI / (131.0 * 180.0); + return true; + + case MPU9150_GYROFSR_500: + m_gyroFsr = fsr; + m_gyroScale = RTMATH_PI / (62.5 * 180.0); + return true; + + case MPU9150_GYROFSR_1000: + m_gyroFsr = fsr; + m_gyroScale = RTMATH_PI / (32.8 * 180.0); + return true; + + case MPU9150_GYROFSR_2000: + m_gyroFsr = fsr; + m_gyroScale = RTMATH_PI / (16.4 * 180.0); + return true; + + default: + HAL_ERROR1("Illegal MPU9150 gyro fsr %d\n", fsr); + return false; + } +} + +bool RTIMUMPU9150::setAccelFsr(unsigned char fsr) +{ + switch (fsr) { + case MPU9150_ACCELFSR_2: + m_accelFsr = fsr; + m_accelScale = 1.0/16384.0; + return true; + + case MPU9150_ACCELFSR_4: + m_accelFsr = fsr; + m_accelScale = 1.0/8192.0; + return true; + + case MPU9150_ACCELFSR_8: + m_accelFsr = fsr; + m_accelScale = 1.0/4096.0; + return true; + + case MPU9150_ACCELFSR_16: + m_accelFsr = fsr; + m_accelScale = 1.0/2048.0; + return true; + + default: + HAL_ERROR1("Illegal MPU9150 accel fsr %d\n", fsr); + return false; + } +} + + +bool RTIMUMPU9150::IMUInit() +{ + unsigned char result; + + m_firstTime = true; + +#ifdef MPU9150_CACHE_MODE + 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_slaveAddr = m_settings->m_I2CSlaveAddress; + + setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate); + setCompassRate(m_settings->m_MPU9150CompassSampleRate); + setLpf(m_settings->m_MPU9150GyroAccelLpf); + setGyroFsr(m_settings->m_MPU9150GyroFsr); + setAccelFsr(m_settings->m_MPU9150AccelFsr); + + setCalibrationData(); + + // enable the I2C bus + + if (!m_settings->HALOpen()) + return false; + + // reset the MPU9150 + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset")) + return false; + + m_settings->delayMs(100); + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset")) + return false; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id")) + return false; + + if (result != MPU9150_ID) { + HAL_ERROR1("Incorrect MPU9150 id %d\n", result); + return false; + } + + // now configure the various components + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf")) + return false; + + if (!setSampleRate()) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr")) + return false; + + // now configure compass + + if (!configureCompass()) + return false; + + // enable the sensors + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2")) + return false; + + // select the data to go into the FIFO and enable + + if (!resetFifo()) + return false; + + gyroBiasInit(); + + HAL_INFO("MPU9150 init complete\n"); + return true; +} + +bool RTIMUMPU9150::configureCompass() +{ + unsigned char asa[3]; + unsigned char id; + + m_compassIs5883 = false; + m_compassDataLength = 8; + + bypassOn(); + + // get fuse ROM data + + if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) { + bypassOff(); + return false; + } + + if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) { + bypassOff(); + return false; + } + + if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "")) { + + // check to see if an HMC5883L is fitted + + if (!m_settings->HALRead(HMC5883_ADDRESS, HMC5883_ID, 1, &id, "Failed to find 5883")) { + bypassOff(); + + // this is returning true so that MPU-6050 by itself will work + + HAL_INFO("Detected MPU-6050 without compass\n"); + + m_imuData.compassValid = false; + return true; + } + if (id != 0x48) { // incorrect id for HMC5883L + + bypassOff(); + + // this is returning true so that MPU-6050 by itself will work + + HAL_INFO("Detected MPU-6050 without compass\n"); + + m_imuData.compassValid = false; + return true; + } + + // HMC5883 is present - use that + + if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_CONFIG_A, 0x38, "Failed to set HMC5883 config A")) { + bypassOff(); + return false; + } + + if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_CONFIG_B, 0x20, "Failed to set HMC5883 config B")) { + bypassOff(); + return false; + } + + if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_MODE, 0x00, "Failed to set HMC5883 mode")) { + bypassOff(); + return false; + } + + HAL_INFO("Detected MPU-6050 with HMC5883\n"); + + m_compassDataLength = 6; + m_compassIs5883 = true; + } else { + + // convert asa to usable scale factor + + m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f; + m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f; + m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f; + + if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) { + bypassOff(); + return false; + } + } + + bypassOff(); + + // now set up MPU9150 to talk to the compass chip + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode")) + return false; + + if (m_compassIs5883) { + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | HMC5883_ADDRESS, "Failed to set slave 0 address")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, HMC5883_DATA_X_HI, "Failed to set slave 0 reg")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x86, "Failed to set slave 0 ctrl")) + return false; + } else { + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO")) + return false; + + } + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc")) + return false; + + if (!setCompassRate()) + return false; + + return true; +} + +bool RTIMUMPU9150::resetFifo() +{ + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_ENABLE, 0, "Writing int enable")) + return false; + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_FIFO_EN, 0, "Writing fifo enable")) + return false; + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 0, "Writing user control")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 0x04, "Resetting fifo")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 0x60, "Enabling the fifo")) + return false; + + m_settings->delayMs(50); + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_ENABLE, 1, "Writing int enable")) + return false; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_FIFO_EN, 0x78, "Failed to set FIFO enables")) + return false; + + return true; +} + +bool RTIMUMPU9150::bypassOn() +{ + unsigned char userControl; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg")) + return false; + + userControl &= ~0x20; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg")) + return false; + + m_settings->delayMs(50); + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x82, "Failed to write int_pin_cfg reg")) + return false; + + m_settings->delayMs(50); + return true; +} + + +bool RTIMUMPU9150::bypassOff() +{ + unsigned char userControl; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg")) + return false; + + userControl |= 0x20; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg")) + return false; + + m_settings->delayMs(50); + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x80, "Failed to write int_pin_cfg reg")) + return false; + + m_settings->delayMs(50); + return true; +} + +bool RTIMUMPU9150::setSampleRate() +{ + int clockRate = 1000; + + if (m_lpf == MPU9150_LPF_256) + clockRate = 8000; + + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_SMPRT_DIV, (unsigned char)(clockRate / m_sampleRate - 1), + "Failed to set sample rate")) + return false; + + return true; +} + +bool RTIMUMPU9150::setCompassRate() +{ + int rate; + + rate = m_sampleRate / m_compassRate - 1; + + if (rate > 31) + rate = 31; + if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV4_CTRL, rate, "Failed to set slave ctrl 4")) + return false; + return true; +} + +int RTIMUMPU9150::IMUGetPollInterval() +{ + return (400 / m_sampleRate); +} + +bool RTIMUMPU9150::IMURead() +{ + unsigned char fifoCount[2]; + unsigned int count; + unsigned char fifoData[12]; + unsigned char compassData[8]; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count")) + return false; + + count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1]; + + if (count == 1024) { + HAL_INFO("MPU9150 fifo has overflowed"); + resetFifo(); + m_imuData.timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp + return false; + } + + +#ifdef MPU9150_CACHE_MODE + if ((m_cacheCount == 0) && (count >= MPU9150_FIFO_CHUNK_SIZE) && (count < (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE))) { + // special case of a small fifo and nothing cached - just handle as simple read + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) + return false; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data")) + return false; + } else { + if (count >= (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE)) { + if (m_cacheCount == MPU9150_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 == MPU9150_CACHE_BLOCK_COUNT) + m_cacheOut = 0; + m_cacheCount--; + } + + int blockCount = count / MPU9150_FIFO_CHUNK_SIZE; // number of chunks in fifo + + if (blockCount > MPU9150_CACHE_SIZE) + blockCount = MPU9150_CACHE_SIZE; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE * blockCount, + m_cache[m_cacheIn].data, "Failed to read fifo data")) + return false; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data")) + return false; + + m_cache[m_cacheIn].count = blockCount; + m_cache[m_cacheIn].index = 0; + + m_cacheCount++; + if (++m_cacheIn == MPU9150_CACHE_BLOCK_COUNT) + m_cacheIn = 0; + + } + + // now fifo has been read if necessary, get something to process + + if (m_cacheCount == 0) + return false; + + memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9150_FIFO_CHUNK_SIZE); + memcpy(compassData, m_cache[m_cacheOut].compass, 8); + + m_cache[m_cacheOut].index += MPU9150_FIFO_CHUNK_SIZE; + + if (--m_cache[m_cacheOut].count == 0) { + // this cache block is now empty + + if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT) + m_cacheOut = 0; + m_cacheCount--; + } + } + +#else + + if (count > MPU9150_FIFO_CHUNK_SIZE * 40) { + // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly + while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) { + if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) + return false; + count -= MPU9150_FIFO_CHUNK_SIZE; + m_imuData.timestamp += m_sampleInterval; + } + } + + if (count < MPU9150_FIFO_CHUNK_SIZE) + return false; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) + return false; + + if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data")) + return false; + +#endif + + RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true); + RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true); + + if (m_compassIs5883) + RTMath::convertToVector(compassData, m_imuData.compass, 0.092f, true); + else + RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.3f, 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()); + + + if (m_compassIs5883) { + // sort out compass axes + + float temp; + + temp = m_imuData.compass.y(); + m_imuData.compass.setY(-m_imuData.compass.z()); + m_imuData.compass.setZ(-temp); + + } else { + + // use the compass fuse data adjustments + + m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]); + m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]); + m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]); + + // sort out compass axes + + float temp; + + temp = m_imuData.compass.x(); + m_imuData.compass.setX(m_imuData.compass.y()); + m_imuData.compass.setY(-temp); + } + + + // now do standard processing + + handleGyroBias(); + calibrateAverageCompass(); + calibrateAccel(); + + if (m_firstTime) + m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); + else + m_imuData.timestamp += m_sampleInterval; + + m_firstTime = false; + + // now update the filter + + updateFusion(); + + return true; +} http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h ---------------------------------------------------------------------- diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h new file mode 100644 index 0000000..396a632 --- /dev/null +++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9150.h @@ -0,0 +1,110 @@ +//////////////////////////////////////////////////////////////////////////// +// +// 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 _RTIMUMPU9150_H +#define _RTIMUMPU9150_H + +#include "RTIMU.h" + +// Define this symbol to use cache mode + +#define MPU9150_CACHE_MODE + +// FIFO transfer size + +#define MPU9150_FIFO_CHUNK_SIZE 12 // gyro and accels take 12 bytes + +#ifdef MPU9150_CACHE_MODE + +// Cache mode defines + +#define MPU9150_CACHE_SIZE 16 // number of chunks in a block +#define MPU9150_CACHE_BLOCK_COUNT 16 // number of cache blocks + +typedef struct +{ + unsigned char data[MPU9150_FIFO_CHUNK_SIZE * MPU9150_CACHE_SIZE]; + int count; // number of chunks in the cache block + int index; // current index into the cache + unsigned char compass[8]; // the raw compass readings for the block + +} MPU9150_CACHE_BLOCK; + +#endif + + +class RTIMUMPU9150 : public RTIMU +{ +public: + RTIMUMPU9150(RTIMUSettings *settings); + ~RTIMUMPU9150(); + + bool setLpf(unsigned char lpf); + bool setSampleRate(int rate); + bool setCompassRate(int rate); + bool setGyroFsr(unsigned char fsr); + bool setAccelFsr(unsigned char fsr); + + virtual const char *IMUName() { return "MPU-9150"; } + virtual int IMUType() { return RTIMU_TYPE_MPU9150; } + virtual bool IMUInit(); + virtual bool IMURead(); + virtual int IMUGetPollInterval(); + +private: + bool configureCompass(); // configures the compass + bool bypassOn(); // talk to compass + bool bypassOff(); // talk to MPU9150 + bool setSampleRate(); + bool setCompassRate(); + bool resetFifo(); + + bool m_firstTime; // if first sample + + unsigned char m_slaveAddr; // I2C address of MPU9150 + + unsigned char m_lpf; // low pass filter setting + int m_compassRate; // compass sample rate in Hz + unsigned char m_gyroFsr; + unsigned char m_accelFsr; + + RTFLOAT m_gyroScale; + RTFLOAT m_accelScale; + + bool m_compassIs5883; // if it is an MPU-6050/HMC5883 combo + int m_compassDataLength; // 8 for MPU-9150, 6 for HMC5883 + RTFLOAT m_compassAdjust[3]; // the compass fuse ROM values converted for use + +#ifdef MPU9150_CACHE_MODE + + MPU9150_CACHE_BLOCK m_cache[MPU9150_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 // _RTIMUMPU9150_H
