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:34 UTC
[4/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/RTIMUMPU9250.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp
new file mode 100644
index 0000000..dc6cc35
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.cpp
@@ -0,0 +1,657 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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)
+
+#include "RTIMUMPU9250.h"
+#include "RTIMUSettings.h"
+
+RTIMUMPU9250::RTIMUMPU9250(RTIMUSettings *settings) : RTIMU(settings)
+{
+
+}
+
+RTIMUMPU9250::~RTIMUMPU9250()
+{
+}
+
+bool RTIMUMPU9250::setSampleRate(int rate)
+{
+ if ((rate < MPU9250_SAMPLERATE_MIN) || (rate > MPU9250_SAMPLERATE_MAX)) {
+ HAL_ERROR1("Illegal sample rate %d\n", rate);
+ return false;
+ }
+
+ // Note: rates interact with the lpf settings
+
+ if ((rate < MPU9250_SAMPLERATE_MAX) && (rate >= 8000))
+ rate = 8000;
+
+ if ((rate < 8000) && (rate >= 1000))
+ rate = 1000;
+
+ if (rate < 1000) {
+ int sampleDiv = (1000 / rate) - 1;
+ m_sampleRate = 1000 / (1 + sampleDiv);
+ } else {
+ m_sampleRate = rate;
+ }
+ m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+ return true;
+}
+
+bool RTIMUMPU9250::setGyroLpf(unsigned char lpf)
+{
+ switch (lpf) {
+ case MPU9250_GYRO_LPF_8800:
+ case MPU9250_GYRO_LPF_3600:
+ case MPU9250_GYRO_LPF_250:
+ case MPU9250_GYRO_LPF_184:
+ case MPU9250_GYRO_LPF_92:
+ case MPU9250_GYRO_LPF_41:
+ case MPU9250_GYRO_LPF_20:
+ case MPU9250_GYRO_LPF_10:
+ case MPU9250_GYRO_LPF_5:
+ m_gyroLpf = lpf;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 gyro lpf %d\n", lpf);
+ return false;
+ }
+}
+
+bool RTIMUMPU9250::setAccelLpf(unsigned char lpf)
+{
+ switch (lpf) {
+ case MPU9250_ACCEL_LPF_1130:
+ case MPU9250_ACCEL_LPF_460:
+ case MPU9250_ACCEL_LPF_184:
+ case MPU9250_ACCEL_LPF_92:
+ case MPU9250_ACCEL_LPF_41:
+ case MPU9250_ACCEL_LPF_20:
+ case MPU9250_ACCEL_LPF_10:
+ case MPU9250_ACCEL_LPF_5:
+ m_accelLpf = lpf;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 accel lpf %d\n", lpf);
+ return false;
+ }
+}
+
+
+bool RTIMUMPU9250::setCompassRate(int rate)
+{
+ if ((rate < MPU9250_COMPASSRATE_MIN) || (rate > MPU9250_COMPASSRATE_MAX)) {
+ HAL_ERROR1("Illegal compass rate %d\n", rate);
+ return false;
+ }
+ m_compassRate = rate;
+ return true;
+}
+
+bool RTIMUMPU9250::setGyroFsr(unsigned char fsr)
+{
+ switch (fsr) {
+ case MPU9250_GYROFSR_250:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (131.0 * 180.0);
+ return true;
+
+ case MPU9250_GYROFSR_500:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (62.5 * 180.0);
+ return true;
+
+ case MPU9250_GYROFSR_1000:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (32.8 * 180.0);
+ return true;
+
+ case MPU9250_GYROFSR_2000:
+ m_gyroFsr = fsr;
+ m_gyroScale = RTMATH_PI / (16.4 * 180.0);
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 gyro fsr %d\n", fsr);
+ return false;
+ }
+}
+
+bool RTIMUMPU9250::setAccelFsr(unsigned char fsr)
+{
+ switch (fsr) {
+ case MPU9250_ACCELFSR_2:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/16384.0;
+ return true;
+
+ case MPU9250_ACCELFSR_4:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/8192.0;
+ return true;
+
+ case MPU9250_ACCELFSR_8:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/4096.0;
+ return true;
+
+ case MPU9250_ACCELFSR_16:
+ m_accelFsr = fsr;
+ m_accelScale = 1.0/2048.0;
+ return true;
+
+ default:
+ HAL_ERROR1("Illegal MPU9250 accel fsr %d\n", fsr);
+ return false;
+ }
+}
+
+
+bool RTIMUMPU9250::IMUInit()
+{
+ unsigned char result;
+
+ m_firstTime = true;
+
+#ifdef MPU9250_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_MPU9250GyroAccelSampleRate);
+ setCompassRate(m_settings->m_MPU9250CompassSampleRate);
+ setGyroLpf(m_settings->m_MPU9250GyroLpf);
+ setAccelLpf(m_settings->m_MPU9250AccelLpf);
+ setGyroFsr(m_settings->m_MPU9250GyroFsr);
+ setAccelFsr(m_settings->m_MPU9250AccelFsr);
+
+ setCalibrationData();
+
+
+ // enable the bus
+
+ if (!m_settings->HALOpen())
+ return false;
+
+ // reset the MPU9250
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x80, "Failed to initiate MPU9250 reset"))
+ return false;
+
+ m_settings->delayMs(100);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x00, "Failed to stop MPU9250 reset"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_WHO_AM_I, 1, &result, "Failed to read MPU9250 id"))
+ return false;
+
+ if (result != MPU9250_ID) {
+ HAL_ERROR2("Incorrect %s id %d\n", IMUName(), result);
+ return false;
+ }
+
+ // now configure the various components
+
+ if (!setGyroConfig())
+ return false;
+
+ if (!setAccelConfig())
+ return false;
+
+ if (!setSampleRate())
+ return false;
+
+ if(!compassSetup()) {
+ return false;
+ }
+
+ if (!setCompassRate())
+ return false;
+
+ // enable the sensors
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_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_INFO1("%s init complete\n", IMUName());
+ return true;
+}
+
+
+bool RTIMUMPU9250::resetFifo()
+{
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_ENABLE, 0, "Writing int enable"))
+ return false;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_FIFO_EN, 0, "Writing fifo enable"))
+ return false;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 0, "Writing user control"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 0x04, "Resetting fifo"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 0x60, "Enabling the fifo"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_ENABLE, 1, "Writing int enable"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_FIFO_EN, 0x78, "Failed to set FIFO enables"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9250::setGyroConfig()
+{
+ unsigned char gyroConfig = m_gyroFsr + ((m_gyroLpf >> 3) & 3);
+ unsigned char gyroLpf = m_gyroLpf & 7;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_GYRO_CONFIG, gyroConfig, "Failed to write gyro config"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_GYRO_LPF, gyroLpf, "Failed to write gyro lpf"))
+ return false;
+ return true;
+}
+
+bool RTIMUMPU9250::setAccelConfig()
+{
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_ACCEL_CONFIG, m_accelFsr, "Failed to write accel config"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_ACCEL_LPF, m_accelLpf, "Failed to write accel lpf"))
+ return false;
+ return true;
+}
+
+bool RTIMUMPU9250::setSampleRate()
+{
+ if (m_sampleRate > 1000)
+ return true; // SMPRT not used above 1000Hz
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_SMPRT_DIV, (unsigned char) (1000 / m_sampleRate - 1),
+ "Failed to set sample rate"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9250::compassSetup() {
+ unsigned char asa[3];
+
+ if (m_settings->m_busIsI2C) {
+ // I2C mode
+
+ bypassOn();
+
+ // get fuse ROM data
+
+ if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0, "Failed to set compass in power down mode 1")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALRead(AK8963_ADDRESS, AK8963_ASAX, 3, asa, "Failed to read compass fuse ROM")) {
+ bypassOff();
+ return false;
+ }
+
+ if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0, "Failed to set compass in power down mode 2")) {
+ bypassOff();
+ return false;
+ }
+
+ bypassOff();
+
+ } else {
+ // SPI mode
+
+ bypassOff();
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS, "Failed to set slave 0 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ASAX, "Failed to set slave 0 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x83, "Failed to set slave 0 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS, "Failed to set slave 1 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL, "Failed to set slave 1 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x00, "Failed to set compass in power down mode 2"))
+ return false;
+
+ m_settings->delayMs(10);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x0f, "Failed to set compass in fuse mode"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 3, asa, "Failed to read compass rom"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x0, "Failed to set compass in power down mode 2"))
+ return false;
+ }
+ // both interfaces
+
+ // 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(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS, "Failed to set slave 0 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ST1, "Failed to set slave 0 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS, "Failed to set slave 1 address"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL, "Failed to set slave 1 reg"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO"))
+ return false;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay"))
+ return false;
+
+ return true;
+}
+
+bool RTIMUMPU9250::setCompassRate()
+{
+ int rate;
+
+ rate = m_sampleRate / m_compassRate - 1;
+
+ if (rate > 31)
+ rate = 31;
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_I2C_SLV4_CTRL, rate, "Failed to set slave ctrl 4"))
+ return false;
+ return true;
+}
+
+
+bool RTIMUMPU9250::bypassOn()
+{
+ unsigned char userControl;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg"))
+ return false;
+
+ userControl &= ~0x20;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x82, "Failed to write int_pin_cfg reg"))
+ return false;
+
+ m_settings->delayMs(50);
+ return true;
+}
+
+
+bool RTIMUMPU9250::bypassOff()
+{
+ unsigned char userControl;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to read user_ctrl reg"))
+ return false;
+
+ userControl |= 0x20;
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_USER_CTRL, 1, &userControl, "Failed to write user_ctrl reg"))
+ return false;
+
+ m_settings->delayMs(50);
+
+ if (!m_settings->HALWrite(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x80, "Failed to write int_pin_cfg reg"))
+ return false;
+
+ m_settings->delayMs(50);
+ return true;
+}
+
+
+int RTIMUMPU9250::IMUGetPollInterval()
+{
+ if (m_sampleRate > 400)
+ return 1;
+ else
+ return (400 / m_sampleRate);
+}
+
+bool RTIMUMPU9250::IMURead()
+{
+ unsigned char fifoCount[2];
+ unsigned int count;
+ unsigned char fifoData[12];
+ unsigned char compassData[8];
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
+ return false;
+
+ count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
+
+ if (count == 512) {
+ HAL_INFO("MPU-9250 fifo has overflowed");
+ resetFifo();
+ m_imuData.timestamp += m_sampleInterval * (512 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
+ return false;
+ }
+
+#ifdef MPU9250_CACHE_MODE
+ if ((m_cacheCount == 0) && (count >= MPU9250_FIFO_CHUNK_SIZE) && (count < (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE))) {
+ // special case of a small fifo and nothing cached - just handle as simple read
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
+ return false;
+ } else {
+ if (count >= (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE)) {
+ if (m_cacheCount == MPU9250_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 == MPU9250_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+
+ int blockCount = count / MPU9250_FIFO_CHUNK_SIZE; // number of chunks in fifo
+
+ if (blockCount > MPU9250_CACHE_SIZE)
+ blockCount = MPU9250_CACHE_SIZE;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE * blockCount,
+ m_cache[m_cacheIn].data, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_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 == MPU9250_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, MPU9250_FIFO_CHUNK_SIZE);
+ memcpy(compassData, m_cache[m_cacheOut].compass, 8);
+
+ m_cache[m_cacheOut].index += MPU9250_FIFO_CHUNK_SIZE;
+
+ if (--m_cache[m_cacheOut].count == 0) {
+ // this cache block is now empty
+
+ if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT)
+ m_cacheOut = 0;
+ m_cacheCount--;
+ }
+ }
+
+#else
+
+ if (count > MPU9250_FIFO_CHUNK_SIZE * 40) {
+ // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
+ while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) {
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+ count -= MPU9250_FIFO_CHUNK_SIZE;
+ m_imuData.timestamp += m_sampleInterval;
+ }
+ }
+
+ if (count < MPU9250_FIFO_CHUNK_SIZE)
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
+ return false;
+
+ if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, 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);
+ RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, 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());
+
+ // 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/RTIMUMPU9250.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h
new file mode 100644
index 0000000..e468e28
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUMPU9250.h
@@ -0,0 +1,118 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUMPU9250_H
+#define _RTIMUMPU9250_H
+
+#include "RTIMU.h"
+
+// Define this symbol to use cache mode
+
+#define MPU9250_CACHE_MODE
+
+// FIFO transfer size
+
+#define MPU9250_FIFO_CHUNK_SIZE 12 // gyro and accels take 12 bytes
+
+#ifdef MPU9250_CACHE_MODE
+
+// Cache mode defines
+
+#define MPU9250_CACHE_SIZE 16 // number of chunks in a block
+#define MPU9250_CACHE_BLOCK_COUNT 16 // number of cache blocks
+
+typedef struct
+{
+ unsigned char data[MPU9250_FIFO_CHUNK_SIZE * MPU9250_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
+
+} MPU9250_CACHE_BLOCK;
+
+#endif
+
+
+class RTIMUMPU9250 : public RTIMU
+{
+public:
+ RTIMUMPU9250(RTIMUSettings *settings);
+ ~RTIMUMPU9250();
+
+ bool setGyroLpf(unsigned char lpf);
+ bool setAccelLpf(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-9250"; }
+ virtual int IMUType() { return RTIMU_TYPE_MPU9250; }
+ virtual bool IMUInit();
+ virtual bool IMURead();
+ virtual int IMUGetPollInterval();
+
+protected:
+
+ RTFLOAT m_compassAdjust[3]; // the compass fuse ROM values converted for use
+
+private:
+ bool setGyroConfig();
+ bool setAccelConfig();
+ bool setSampleRate();
+ bool compassSetup();
+ bool setCompassRate();
+ bool resetFifo();
+ bool bypassOn();
+ bool bypassOff();
+
+ bool m_firstTime; // if first sample
+
+ unsigned char m_slaveAddr; // I2C address of MPU9150
+
+ unsigned char m_gyroLpf; // gyro low pass filter setting
+ unsigned char m_accelLpf; // accel 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;
+
+
+#ifdef MPU9250_CACHE_MODE
+
+ MPU9250_CACHE_BLOCK m_cache[MPU9250_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 // _RTIMUMPU9250_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp
new file mode 100644
index 0000000..b497dc5
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.cpp
@@ -0,0 +1,54 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTIMUNull.h"
+#include "RTIMUSettings.h"
+
+RTIMUNull::RTIMUNull(RTIMUSettings *settings) : RTIMU(settings)
+{
+}
+
+RTIMUNull::~RTIMUNull()
+{
+}
+
+bool RTIMUNull::IMUInit()
+{
+ return true;
+}
+
+int RTIMUNull::IMUGetPollInterval()
+{
+ return (100); // just a dummy value really
+}
+
+bool RTIMUNull::IMURead()
+{
+ updateFusion();
+ return true;
+}
+
+void RTIMUNull::setIMUData(const RTIMU_DATA& data)
+{
+ m_imuData = data;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h
new file mode 100644
index 0000000..45375d0
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTIMUNull.h
@@ -0,0 +1,58 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTIMUNULL_H
+#define _RTIMUNULL_H
+
+// IMUNull is a dummy IMU that assumes sensor data is coming from elsewhere,
+// for example, across a network.
+//
+// Call IMUInit in the normal way. Then for every update, call setIMUData and then IMURead
+// to kick the kalman filter.
+
+#include "RTIMU.h"
+
+class RTIMUSettings;
+
+class RTIMUNull : public RTIMU
+{
+public:
+ RTIMUNull(RTIMUSettings *settings);
+ ~RTIMUNull();
+
+ // The timestamp parameter is assumed to be from RTMath::currentUSecsSinceEpoch()
+
+ void setIMUData(const RTIMU_DATA& data);
+
+ virtual const char *IMUName() { return "Null IMU"; }
+ virtual int IMUType() { return RTIMU_TYPE_NULL; }
+ virtual bool IMUInit();
+ virtual int IMUGetPollInterval();
+ virtual bool IMURead();
+ virtual bool IMUGyroBiasValid() { return true; }
+
+private:
+ uint64_t m_timestamp;
+};
+
+#endif // _RTIMUNULL_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp
new file mode 100644
index 0000000..6074386
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.cpp
@@ -0,0 +1,70 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressure.h"
+
+#include "RTPressureBMP180.h"
+#include "RTPressureLPS25H.h"
+#include "RTPressureMS5611.h"
+#include "RTPressureMS5637.h"
+
+RTPressure *RTPressure::createPressure(RTIMUSettings *settings)
+{
+ switch (settings->m_pressureType) {
+ case RTPRESSURE_TYPE_BMP180:
+ return new RTPressureBMP180(settings);
+
+ case RTPRESSURE_TYPE_LPS25H:
+ return new RTPressureLPS25H(settings);
+
+ case RTPRESSURE_TYPE_MS5611:
+ return new RTPressureMS5611(settings);
+
+ case RTPRESSURE_TYPE_MS5637:
+ return new RTPressureMS5637(settings);
+
+ case RTPRESSURE_TYPE_AUTODISCOVER:
+ if (settings->discoverPressure(settings->m_pressureType, settings->m_I2CPressureAddress)) {
+ settings->saveSettings();
+ return RTPressure::createPressure(settings);
+ }
+ return NULL;
+
+ case RTPRESSURE_TYPE_NULL:
+ return NULL;
+
+ default:
+ return NULL;
+ }
+}
+
+
+RTPressure::RTPressure(RTIMUSettings *settings)
+{
+ m_settings = settings;
+}
+
+RTPressure::~RTPressure()
+{
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h
new file mode 100644
index 0000000..aa95553
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressure.h
@@ -0,0 +1,55 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSURE_H
+#define _RTPRESSURE_H
+
+#include "RTIMUSettings.h"
+#include "RTIMULibDefs.h"
+#include "RTPressureDefs.h"
+
+class RTPressure
+{
+public:
+ // Pressure sensor objects should always be created with the following call
+
+ static RTPressure *createPressure(RTIMUSettings *settings);
+
+ // Constructor/destructor
+
+ RTPressure(RTIMUSettings *settings);
+ virtual ~RTPressure();
+
+ // These functions must be provided by sub classes
+
+ virtual const char *pressureName() = 0; // the name of the pressure sensor
+ virtual int pressureType() = 0; // the type code of the pressure sensor
+ virtual bool pressureInit() = 0; // set up the pressure sensor
+ virtual bool pressureRead(RTIMU_DATA& data) = 0; // get latest value
+
+protected:
+ RTIMUSettings *m_settings; // the settings object pointer
+
+};
+
+#endif // _RTPRESSURE_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp
new file mode 100644
index 0000000..e2a5a87
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.cpp
@@ -0,0 +1,230 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureBMP180.h"
+
+RTPressureBMP180::RTPressureBMP180(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_validReadings = false;
+}
+
+RTPressureBMP180::~RTPressureBMP180()
+{
+}
+
+bool RTPressureBMP180::pressureInit()
+{
+ unsigned char result;
+ unsigned char data[22];
+
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ // check ID of chip
+
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_ID, 1, &result, "Failed to read BMP180 id"))
+ return false;
+
+ if (result != BMP180_ID) {
+ HAL_ERROR1("Incorrect BMP180 id %d\n", result);
+ return false;
+ }
+
+ // get calibration data
+
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_AC1, 22, data, "Failed to read BMP180 calibration data"))
+ return false;
+
+ m_AC1 = (int16_t)(((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+ m_AC2 = (int16_t)(((uint16_t)data[2]) << 8) + (uint16_t)data[3];
+ m_AC3 = (int16_t)(((uint16_t)data[4]) << 8) + (uint16_t)data[4];
+ m_AC4 = (((uint16_t)data[6]) << 8) + (uint16_t)data[7];
+ m_AC5 = (((uint16_t)data[8]) << 8) + (uint16_t)data[9];
+ m_AC6 = (((uint16_t)data[10]) << 8) + (uint16_t)data[11];
+ m_B1 = (int16_t)(((uint16_t)data[12]) << 8) + (uint16_t)data[13];
+ m_B2 = (int16_t)(((uint16_t)data[14]) << 8) + (uint16_t)data[15];
+ m_MB = (int16_t)(((uint16_t)data[16]) << 8) + (uint16_t)data[17];
+ m_MC = (int16_t)(((uint16_t)data[18]) << 8) + (uint16_t)data[19];
+ m_MD = (int16_t)(((uint16_t)data[20]) << 8) + (uint16_t)data[21];
+
+ m_state = BMP180_STATE_IDLE;
+ m_oss = BMP180_SCO_PRESSURECONV_ULP;
+ return true;
+}
+
+bool RTPressureBMP180::pressureRead(RTIMU_DATA& data)
+{
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (m_state == BMP180_STATE_IDLE) {
+ // start a temperature conversion
+ if (!m_settings->HALWrite(m_pressureAddr, BMP180_REG_SCO, BMP180_SCO_TEMPCONV, "Failed to start temperature conversion")) {
+ return false;
+ } else {
+ m_state = BMP180_STATE_TEMPERATURE;
+ }
+ }
+
+ pressureBackground();
+
+ if (m_validReadings) {
+ data.pressureValid = true;
+ data.temperatureValid = true;
+ data.temperature = m_temperature;
+ data.pressure = m_pressure;
+ // printf("P: %f, T: %f\n", m_pressure, m_temperature);
+ }
+ return true;
+}
+
+
+void RTPressureBMP180::pressureBackground()
+{
+ uint8_t data[2];
+
+ switch (m_state) {
+ case BMP180_STATE_IDLE:
+ break;
+
+ case BMP180_STATE_TEMPERATURE:
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_SCO, 1, data, "Failed to read BMP180 temp conv status")) {
+ break;
+ }
+ if ((data[0] & 0x20) == 0x20)
+ break; // conversion not finished
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_RESULT, 2, data, "Failed to read BMP180 temp conv result")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+ m_rawTemperature = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+
+ data[0] = 0x34 + (m_oss << 6);
+ if (!m_settings->HALWrite(m_pressureAddr, BMP180_REG_SCO, 1, data, "Failed to start pressure conversion")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+ m_state = BMP180_STATE_PRESSURE;
+ break;
+
+ case BMP180_STATE_PRESSURE:
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_SCO, 1, data, "Failed to read BMP180 pressure conv status")) {
+ break;
+ }
+ if ((data[0] & 0x20) == 0x20)
+ break; // conversion not finished
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_RESULT, 2, data, "Failed to read BMP180 temp conv result")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+ m_rawPressure = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+
+ if (!m_settings->HALRead(m_pressureAddr, BMP180_REG_XLSB, 1, data, "Failed to read BMP180 XLSB")) {
+ m_state = BMP180_STATE_IDLE;
+ break;
+ }
+
+ // call this function for testing only
+ // should give T = 150 (15.0C) and pressure 6996 (699.6hPa)
+
+ // setTestData();
+
+ int32_t pressure = ((((uint32_t)(m_rawPressure)) << 8) + (uint32_t)(data[0])) >> (8 - m_oss);
+
+ m_state = BMP180_STATE_IDLE;
+
+ // calculate compensated temperature
+
+ int32_t X1 = (((int32_t)m_rawTemperature - m_AC6) * m_AC5) / 32768;
+
+ if ((X1 + m_MD) == 0) {
+ break;
+ }
+
+ int32_t X2 = (m_MC * 2048) / (X1 + m_MD);
+ int32_t B5 = X1 + X2;
+ m_temperature = (RTFLOAT)((B5 + 8) / 16) / (RTFLOAT)10;
+
+ // calculate compensated pressure
+
+ int32_t B6 = B5 - 4000;
+ // printf("B6 = %d\n", B6);
+ X1 = (m_B2 * ((B6 * B6) / 4096)) / 2048;
+ // printf("X1 = %d\n", X1);
+ X2 = (m_AC2 * B6) / 2048;
+ // printf("X2 = %d\n", X2);
+ int32_t X3 = X1 + X2;
+ // printf("X3 = %d\n", X3);
+ int32_t B3 = (((m_AC1 * 4 + X3) << m_oss) + 2) / 4;
+ // printf("B3 = %d\n", B3);
+ X1 = (m_AC3 * B6) / 8192;
+ // printf("X1 = %d\n", X1);
+ X2 = (m_B1 * ((B6 * B6) / 4096)) / 65536;
+ // printf("X2 = %d\n", X2);
+ X3 = ((X1 + X2) + 2) / 4;
+ // printf("X3 = %d\n", X3);
+ int32_t B4 = (m_AC4 * (unsigned long)(X3 + 32768)) / 32768;
+ // printf("B4 = %d\n", B4);
+ uint32_t B7 = ((unsigned long)pressure - B3) * (50000 >> m_oss);
+ // printf("B7 = %d\n", B7);
+
+ int32_t p;
+ if (B7 < 0x80000000)
+ p = (B7 * 2) / B4;
+ else
+ p = (B7 / B4) * 2;
+
+ // printf("p = %d\n", p);
+ X1 = (p / 256) * (p / 256);
+ // printf("X1 = %d\n", X1);
+ X1 = (X1 * 3038) / 65536;
+ // printf("X1 = %d\n", X1);
+ X2 = (-7357 * p) / 65536;
+ // printf("X2 = %d\n", X2);
+ m_pressure = (RTFLOAT)(p + (X1 + X2 + 3791) / 16) / (RTFLOAT)100; // the extra 100 factor is to get 1hPa units
+
+ m_validReadings = true;
+
+ // printf("UP = %d, P = %f, UT = %d, T = %f\n", m_rawPressure, m_pressure, m_rawTemperature, m_temperature);
+ break;
+ }
+}
+
+void RTPressureBMP180::setTestData()
+{
+ m_AC1 = 408;
+ m_AC2 = -72;
+ m_AC3 = -14383;
+ m_AC4 = 32741;
+ m_AC5 = 32757;
+ m_AC6 = 23153;
+ m_B1 = 6190;
+ m_B2 = 4;
+ m_MB = -32767;
+ m_MC = -8711;
+ m_MD = 2868;
+
+ m_rawTemperature = 27898;
+ m_rawPressure = 23843;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h
new file mode 100644
index 0000000..b1fb2e9
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureBMP180.h
@@ -0,0 +1,88 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREBMP180_H_
+#define _RTPRESSUREBMP180_H_
+
+#include "RTPressure.h"
+
+// State definitions
+
+#define BMP180_STATE_IDLE 0
+#define BMP180_STATE_TEMPERATURE 1
+#define BMP180_STATE_PRESSURE 2
+
+// Conversion reg defs
+
+#define BMP180_SCO_TEMPCONV 0x2e // temperature conversion
+#define BMP180_SCO_PRESSURECONV_ULP 0 // ultra low power pressure conversion
+#define BMP180_SCO_PRESSURECONV_STD 1 // standard pressure conversion
+#define BMP180_SCO_PRESSURECONV_HR 2 // high res pressure conversion
+#define BMP180_SCO_PRESSURECONV_UHR 3 // ultra high res pressure conversion
+
+class RTIMUSettings;
+
+class RTPressureBMP180 : public RTPressure
+{
+public:
+ RTPressureBMP180(RTIMUSettings *settings);
+ ~RTPressureBMP180();
+
+ virtual const char *pressureName() { return "BMP180"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_BMP180; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ void pressureBackground();
+ void setTestData();
+
+ unsigned char m_pressureAddr; // I2C address
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+
+ // This is the calibration data read from the sensor
+
+ int32_t m_AC1;
+ int32_t m_AC2;
+ int32_t m_AC3;
+ uint32_t m_AC4;
+ uint32_t m_AC5;
+ uint32_t m_AC6;
+ int32_t m_B1;
+ int32_t m_B2;
+ int32_t m_MB;
+ int32_t m_MC;
+ int32_t m_MD;
+
+ int m_state;
+ int m_oss;
+
+ uint16_t m_rawPressure;
+ uint16_t m_rawTemperature;
+
+ bool m_validReadings;
+};
+
+#endif // _RTPRESSUREBMP180_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h
new file mode 100644
index 0000000..6ece64f
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureDefs.h
@@ -0,0 +1,105 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREDEFS_H
+#define _RTPRESSUREDEFS_H
+
+// Pressure sensor type codes
+
+#define RTPRESSURE_TYPE_AUTODISCOVER 0 // audodiscover the pressure sensor
+#define RTPRESSURE_TYPE_NULL 1 // if no physical hardware
+#define RTPRESSURE_TYPE_BMP180 2 // BMP180
+#define RTPRESSURE_TYPE_LPS25H 3 // LPS25H
+#define RTPRESSURE_TYPE_MS5611 4 // MS5611
+#define RTPRESSURE_TYPE_MS5637 5 // MS5637
+
+//----------------------------------------------------------
+//
+// BMP180
+
+// BMP180 I2C Slave Addresses
+
+#define BMP180_ADDRESS 0x77
+#define BMP180_REG_ID 0xd0
+#define BMP180_ID 0x55
+
+// Register map
+
+#define BMP180_REG_AC1 0xaa
+#define BMP180_REG_SCO 0xf4
+#define BMP180_REG_RESULT 0xf6
+#define BMP180_REG_XLSB 0xf8
+
+//----------------------------------------------------------
+//
+// LPS25H
+
+// LPS25H I2C Slave Addresses
+
+#define LPS25H_ADDRESS0 0x5c
+#define LPS25H_ADDRESS1 0x5d
+#define LPS25H_REG_ID 0x0f
+#define LPS25H_ID 0xbd
+
+// Register map
+
+#define LPS25H_REF_P_XL 0x08
+#define LPS25H_REF_P_XH 0x09
+#define LPS25H_RES_CONF 0x10
+#define LPS25H_CTRL_REG_1 0x20
+#define LPS25H_CTRL_REG_2 0x21
+#define LPS25H_CTRL_REG_3 0x22
+#define LPS25H_CTRL_REG_4 0x23
+#define LPS25H_INT_CFG 0x24
+#define LPS25H_INT_SOURCE 0x25
+#define LPS25H_STATUS_REG 0x27
+#define LPS25H_PRESS_OUT_XL 0x28
+#define LPS25H_PRESS_OUT_L 0x29
+#define LPS25H_PRESS_OUT_H 0x2a
+#define LPS25H_TEMP_OUT_L 0x2b
+#define LPS25H_TEMP_OUT_H 0x2c
+#define LPS25H_FIFO_CTRL 0x2e
+#define LPS25H_FIFO_STATUS 0x2f
+#define LPS25H_THS_P_L 0x30
+#define LPS25H_THS_P_H 0x31
+#define LPS25H_RPDS_L 0x39
+#define LPS25H_RPDS_H 0x3a
+
+//----------------------------------------------------------
+//
+// MS5611 and MS5637
+
+// MS5611 I2C Slave Addresses
+
+#define MS5611_ADDRESS0 0x76
+#define MS5611_ADDRESS1 0x77
+
+// commands
+
+#define MS5611_CMD_RESET 0x1e
+#define MS5611_CMD_CONV_D1 0x48
+#define MS5611_CMD_CONV_D2 0x58
+#define MS5611_CMD_PROM 0xa0
+#define MS5611_CMD_ADC 0x00
+
+#endif // _RTPRESSUREDEFS_H
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp
new file mode 100644
index 0000000..79cc813
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.cpp
@@ -0,0 +1,91 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureLPS25H.h"
+#include "RTPressureDefs.h"
+
+RTPressureLPS25H::RTPressureLPS25H(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_pressureValid = false;
+ m_temperatureValid = false;
+ }
+
+RTPressureLPS25H::~RTPressureLPS25H()
+{
+}
+
+bool RTPressureLPS25H::pressureInit()
+{
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_CTRL_REG_1, 0xc4, "Failed to set LPS25H CTRL_REG_1"))
+ return false;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_RES_CONF, 0x05, "Failed to set LPS25H RES_CONF"))
+ return false;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_FIFO_CTRL, 0xc0, "Failed to set LPS25H FIFO_CTRL"))
+ return false;
+
+ if (!m_settings->HALWrite(m_pressureAddr, LPS25H_CTRL_REG_2, 0x40, "Failed to set LPS25H CTRL_REG_2"))
+ return false;
+
+ return true;
+}
+
+
+bool RTPressureLPS25H::pressureRead(RTIMU_DATA& data)
+{
+ unsigned char rawData[3];
+ unsigned char status;
+
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (!m_settings->HALRead(m_pressureAddr, LPS25H_STATUS_REG, 1, &status, "Failed to read LPS25H status"))
+ return false;
+
+ if (status & 2) {
+ if (!m_settings->HALRead(m_pressureAddr, LPS25H_PRESS_OUT_XL + 0x80, 3, rawData, "Failed to read LPS25H pressure"))
+ return false;
+
+ m_pressure = (RTFLOAT)((((unsigned int)rawData[2]) << 16) | (((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]) / (RTFLOAT)4096;
+ m_pressureValid = true;
+ }
+ if (status & 1) {
+ if (!m_settings->HALRead(m_pressureAddr, LPS25H_TEMP_OUT_L + 0x80, 2, rawData, "Failed to read LPS25H temperature"))
+ return false;
+
+ m_temperature = (int16_t)((((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]) / (RTFLOAT)480 + (RTFLOAT)42.5;
+ m_temperatureValid = true;
+ }
+
+ data.pressureValid = m_pressureValid;
+ data.pressure = m_pressure;
+ data.temperatureValid = m_temperatureValid;
+ data.temperature = m_temperature;
+
+ return true;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h
new file mode 100644
index 0000000..2b4a1a9
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureLPS25H.h
@@ -0,0 +1,53 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSURELPS25H_H_
+#define _RTPRESSURELPS25H_H_
+
+#include "RTPressure.h"
+
+class RTIMUSettings;
+
+class RTPressureLPS25H : public RTPressure
+{
+public:
+ RTPressureLPS25H(RTIMUSettings *settings);
+ ~RTPressureLPS25H();
+
+ virtual const char *pressureName() { return "LPS25H"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_LPS25H; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ unsigned char m_pressureAddr; // I2C address
+
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+ bool m_pressureValid;
+ bool m_temperatureValid;
+
+};
+
+#endif // _RTPRESSURELPS25H_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
new file mode 100644
index 0000000..3aa8824
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.cpp
@@ -0,0 +1,170 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureMS5611.h"
+
+RTPressureMS5611::RTPressureMS5611(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_validReadings = false;
+}
+
+RTPressureMS5611::~RTPressureMS5611()
+{
+}
+
+bool RTPressureMS5611::pressureInit()
+{
+ unsigned char cmd = MS5611_CMD_PROM + 2;
+ unsigned char data[2];
+
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ // get calibration data
+
+ for (int i = 0; i < 6; i++) {
+ if (!m_settings->HALRead(m_pressureAddr, cmd, 2, data, "Failed to read MS5611 calibration data"))
+ return false;
+ m_calData[i] = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+ // printf("Cal index: %d, data: %d\n", i, m_calData[i]);
+ cmd += 2;
+ }
+
+ m_state = MS5611_STATE_IDLE;
+ return true;
+}
+
+bool RTPressureMS5611::pressureRead(RTIMU_DATA& data)
+{
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (m_state == MS5611_STATE_IDLE) {
+ // start pressure conversion
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D1, 0, 0, "Failed to start MS5611 pressure conversion")) {
+ return false;
+ } else {
+ m_state = MS5611_STATE_PRESSURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ }
+
+ pressureBackground();
+
+ if (m_validReadings) {
+ data.pressureValid = true;
+ data.temperatureValid = true;
+ data.temperature = m_temperature;
+ data.pressure = m_pressure;
+ }
+ return true;
+}
+
+
+void RTPressureMS5611::pressureBackground()
+{
+ uint8_t data[3];
+
+ switch (m_state) {
+ case MS5611_STATE_IDLE:
+ break;
+
+ case MS5611_STATE_PRESSURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 pressure")) {
+ break;
+ }
+ m_D1 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+
+ // start temperature conversion
+
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D2, 0, 0, "Failed to start MS5611 temperature conversion")) {
+ break;
+ } else {
+ m_state = MS5611_STATE_TEMPERATURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ break;
+
+ case MS5611_STATE_TEMPERATURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 temperature")) {
+ break;
+ }
+ m_D2 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+
+ // call this function for testing only
+ // should give T = 2007 (20.07C) and pressure 100009 (1000.09hPa)
+
+ // setTestData();
+
+ // now calculate the real values
+
+ int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
+
+ int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this needs to be divided by 100
+
+ int64_t offset = ((int64_t)m_calData[1] << 16) + (((int64_t)m_calData[3] * deltaT) >> 7);
+ int64_t sens = ((int64_t)m_calData[0] << 15) + (((int64_t)m_calData[2] * deltaT) >> 8);
+
+ // do second order temperature compensation
+
+ if (temperature < 2000) {
+ int64_t T2 = (deltaT * deltaT) >> 31;
+ int64_t offset2 = 5 * ((temperature - 2000) * (temperature - 2000)) / 2;
+ int64_t sens2 = offset2 / 2;
+ if (temperature < -1500) {
+ offset2 += 7 * (temperature + 1500) * (temperature + 1500);
+ sens2 += 11 * ((temperature + 1500) * (temperature + 1500)) / 2;
+ }
+ temperature -= T2;
+ offset -= offset2;
+ sens -=sens2;
+ }
+
+ m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+ m_temperature = (RTFLOAT)temperature/(RTFLOAT)100;
+
+ // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);
+
+ m_validReadings = true;
+ m_state = MS5611_STATE_IDLE;
+ break;
+ }
+}
+
+void RTPressureMS5611::setTestData()
+{
+ m_calData[0] = 40127;
+ m_calData[1] = 36924;
+ m_calData[2] = 23317;
+ m_calData[3] = 23282;
+ m_calData[4] = 33464;
+ m_calData[5] = 28312;
+
+ m_D1 = 9085466;
+ m_D2 = 8569150;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h
new file mode 100644
index 0000000..ab83c14
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5611.h
@@ -0,0 +1,69 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREMS5611_H_
+#define _RTPRESSUREMS5611_H_
+
+#include "RTPressure.h"
+
+// State definitions
+
+#define MS5611_STATE_IDLE 0
+#define MS5611_STATE_TEMPERATURE 1
+#define MS5611_STATE_PRESSURE 2
+
+class RTIMUSettings;
+
+class RTPressureMS5611 : public RTPressure
+{
+public:
+ RTPressureMS5611(RTIMUSettings *settings);
+ ~RTPressureMS5611();
+
+ virtual const char *pressureName() { return "MS5611"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_MS5611; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ void pressureBackground();
+ void setTestData();
+
+ unsigned char m_pressureAddr; // I2C address
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+
+ int m_state;
+
+ uint16_t m_calData[6]; // calibration data
+
+ uint32_t m_D1;
+ uint32_t m_D2;
+
+ uint64_t m_timer; // used to time coversions
+
+ bool m_validReadings;
+};
+
+#endif // _RTPRESSUREMS5611_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
new file mode 100644
index 0000000..018bbe0
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.cpp
@@ -0,0 +1,172 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTPressureMS5637.h"
+
+RTPressureMS5637::RTPressureMS5637(RTIMUSettings *settings) : RTPressure(settings)
+{
+ m_validReadings = false;
+}
+
+RTPressureMS5637::~RTPressureMS5637()
+{
+}
+
+bool RTPressureMS5637::pressureInit()
+{
+ unsigned char cmd = MS5611_CMD_PROM + 2;
+ unsigned char data[2];
+
+ m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+ // get calibration data
+
+ for (int i = 0; i < 6; i++) {
+ if (!m_settings->HALRead(m_pressureAddr, cmd, 2, data, "Failed to read MS5611 calibration data"))
+ return false;
+ m_calData[i] = (((uint16_t)data[0]) << 8) | ((uint16_t)data[1]);
+ // printf("Cal index: %d, data: %d\n", i, m_calData[i]);
+ cmd += 2;
+ }
+
+ m_state = MS5637_STATE_IDLE;
+ return true;
+}
+
+bool RTPressureMS5637::pressureRead(RTIMU_DATA& data)
+{
+ data.pressureValid = false;
+ data.temperatureValid = false;
+ data.temperature = 0;
+ data.pressure = 0;
+
+ if (m_state == MS5637_STATE_IDLE) {
+ // start pressure conversion
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D1, 0, 0, "Failed to start MS5611 pressure conversion")) {
+ return false;
+ } else {
+ m_state = MS5637_STATE_PRESSURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ }
+
+ pressureBackground();
+
+ if (m_validReadings) {
+ data.pressureValid = true;
+ data.temperatureValid = true;
+ data.temperature = m_temperature;
+ data.pressure = m_pressure;
+ }
+ return true;
+}
+
+
+void RTPressureMS5637::pressureBackground()
+{
+ uint8_t data[3];
+
+ switch (m_state) {
+ case MS5637_STATE_IDLE:
+ break;
+
+ case MS5637_STATE_PRESSURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 pressure")) {
+ break;
+ }
+ m_D1 = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
+
+ // start temperature conversion
+
+ if (!m_settings->HALWrite(m_pressureAddr, MS5611_CMD_CONV_D2, 0, 0, "Failed to start MS5611 temperature conversion")) {
+ break;
+ } else {
+ m_state = MS5637_STATE_TEMPERATURE;
+ m_timer = RTMath::currentUSecsSinceEpoch();
+ }
+ break;
+
+ case MS5637_STATE_TEMPERATURE:
+ if ((RTMath::currentUSecsSinceEpoch() - m_timer) < 10000)
+ break; // not time yet
+ if (!m_settings->HALRead(m_pressureAddr, MS5611_CMD_ADC, 3, data, "Failed to read MS5611 temperature")) {
+ break;
+ }
+ m_D2 = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
+
+ // call this function for testing only
+ // should give T = 2000 (20.00C) and pressure 110002 (1100.02hPa)
+
+ // setTestData();
+
+ // now calculate the real values
+
+ int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
+
+ int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this still needs to be divided by 100
+
+ int64_t offset = (((int64_t)m_calData[1]) << 17) + ((m_calData[3] * deltaT) >> 6);
+ int64_t sens = (((int64_t)m_calData[0]) << 16) + ((m_calData[2] * deltaT) >> 7);
+
+ // do second order temperature compensation
+
+ if (temperature < 2000) {
+ int64_t T2 = (3 * (deltaT * deltaT)) >> 33;
+ int64_t offset2 = 61 * ((temperature - 2000) * (temperature - 2000)) / 16;
+ int64_t sens2 = 29 * ((temperature - 2000) * (temperature - 2000)) / 16;
+ if (temperature < -1500) {
+ offset2 += 17 * (temperature + 1500) * (temperature + 1500);
+ sens2 += 9 * ((temperature + 1500) * (temperature + 1500));
+ }
+ temperature -= T2;
+ offset -= offset2;
+ sens -=sens2;
+ } else {
+ temperature -= (5 * (deltaT * deltaT)) >> 38;
+ }
+
+ m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+ m_temperature = (RTFLOAT)temperature/(RTFLOAT)100;
+
+ // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);
+
+ m_validReadings = true;
+ m_state = MS5637_STATE_IDLE;
+ break;
+ }
+}
+
+void RTPressureMS5637::setTestData()
+{
+ m_calData[0] = 46372;
+ m_calData[1] = 43981;
+ m_calData[2] = 29059;
+ m_calData[3] = 27842;
+ m_calData[4] = 31553;
+ m_calData[5] = 28165;
+
+ m_D1 = 6465444;
+ m_D2 = 8077636;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h
new file mode 100644
index 0000000..3320d5d
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/IMUDrivers/RTPressureMS5637.h
@@ -0,0 +1,69 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTPRESSUREMS5637_H_
+#define _RTPRESSUREMS5637_H_
+
+#include "RTPressure.h"
+
+// State definitions
+
+#define MS5637_STATE_IDLE 0
+#define MS5637_STATE_TEMPERATURE 1
+#define MS5637_STATE_PRESSURE 2
+
+class RTIMUSettings;
+
+class RTPressureMS5637 : public RTPressure
+{
+public:
+ RTPressureMS5637(RTIMUSettings *settings);
+ ~RTPressureMS5637();
+
+ virtual const char *pressureName() { return "MS5637"; }
+ virtual int pressureType() { return RTPRESSURE_TYPE_MS5611; }
+ virtual bool pressureInit();
+ virtual bool pressureRead(RTIMU_DATA& data);
+
+private:
+ void pressureBackground();
+ void setTestData();
+
+ unsigned char m_pressureAddr; // I2C address
+ RTFLOAT m_pressure; // the current pressure
+ RTFLOAT m_temperature; // the current temperature
+
+ int m_state;
+
+ uint16_t m_calData[6]; // calibration data
+
+ uint32_t m_D1;
+ uint32_t m_D2;
+
+ uint64_t m_timer; // used to time coversions
+
+ bool m_validReadings;
+};
+
+#endif // _RTPRESSUREMS5637_H_
+
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp b/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp
new file mode 100644
index 0000000..362b38c
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTFusion.cpp
@@ -0,0 +1,137 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 "RTFusion.h"
+#include "RTIMUHal.h"
+
+// The slerp power valule controls the influence of the measured state to correct the predicted state
+// 0 = measured state ignored (just gyros), 1 = measured state overrides predicted state.
+// In between 0 and 1 mixes the two conditions
+
+#define RTQF_SLERP_POWER (RTFLOAT)0.02;
+
+const char *RTFusion::m_fusionNameMap[] = {
+ "NULL",
+ "Kalman STATE4",
+ "RTQF"};
+
+RTFusion::RTFusion()
+{
+ m_debug = false;
+ m_firstTime = true;
+ m_enableGyro = true;
+ m_enableAccel = true;
+ m_enableCompass = true;
+
+ m_gravity.setScalar(0);
+ m_gravity.setX(0);
+ m_gravity.setY(0);
+ m_gravity.setZ(1);
+
+ m_slerpPower = RTQF_SLERP_POWER;
+}
+
+RTFusion::~RTFusion()
+{
+}
+
+void RTFusion::calculatePose(const RTVector3& accel, const RTVector3& mag, float magDeclination)
+{
+ RTQuaternion m;
+ RTQuaternion q;
+
+ if (m_enableAccel) {
+ accel.accelToEuler(m_measuredPose);
+ } else {
+ m_measuredPose = m_fusionPose;
+ m_measuredPose.setZ(0);
+ }
+
+ if (m_enableCompass && m_compassValid) {
+ q.fromEuler(m_measuredPose);
+ m.setScalar(0);
+ m.setX(mag.x());
+ m.setY(mag.y());
+ m.setZ(mag.z());
+
+ m = q * m * q.conjugate();
+ m_measuredPose.setZ(-atan2(m.y(), m.x()) - magDeclination);
+ } else {
+ m_measuredPose.setZ(m_fusionPose.z());
+ }
+
+ m_measuredQPose.fromEuler(m_measuredPose);
+
+ // check for quaternion aliasing. If the quaternion has the wrong sign
+ // the kalman filter will be very unhappy.
+
+ int maxIndex = -1;
+ RTFLOAT maxVal = -1000;
+
+ for (int i = 0; i < 4; i++) {
+ if (fabs(m_measuredQPose.data(i)) > maxVal) {
+ maxVal = fabs(m_measuredQPose.data(i));
+ maxIndex = i;
+ }
+ }
+
+ // if the biggest component has a different sign in the measured and kalman poses,
+ // change the sign of the measured pose to match.
+
+ if (((m_measuredQPose.data(maxIndex) < 0) && (m_fusionQPose.data(maxIndex) > 0)) ||
+ ((m_measuredQPose.data(maxIndex) > 0) && (m_fusionQPose.data(maxIndex) < 0))) {
+ m_measuredQPose.setScalar(-m_measuredQPose.scalar());
+ m_measuredQPose.setX(-m_measuredQPose.x());
+ m_measuredQPose.setY(-m_measuredQPose.y());
+ m_measuredQPose.setZ(-m_measuredQPose.z());
+ m_measuredQPose.toEuler(m_measuredPose);
+ }
+}
+
+
+RTVector3 RTFusion::getAccelResiduals()
+{
+ RTQuaternion rotatedGravity;
+ RTQuaternion fusedConjugate;
+ RTQuaternion qTemp;
+ RTVector3 residuals;
+
+ // do gravity rotation and subtraction
+
+ // create the conjugate of the pose
+
+ fusedConjugate = m_fusionQPose.conjugate();
+
+ // now do the rotation - takes two steps with qTemp as the intermediate variable
+
+ qTemp = m_gravity * m_fusionQPose;
+ rotatedGravity = fusedConjugate * qTemp;
+
+ // now adjust the measured accel and change the signs to make sense
+
+ residuals.setX(-(m_accel.x() - rotatedGravity.x()));
+ residuals.setY(-(m_accel.y() - rotatedGravity.y()));
+ residuals.setZ(-(m_accel.z() - rotatedGravity.z()));
+ return residuals;
+}
http://git-wip-us.apache.org/repos/asf/nifi-minifi-cpp/blob/9dbad3bd/thirdparty/RTIMULib/RTIMULib/RTFusion.h
----------------------------------------------------------------------
diff --git a/thirdparty/RTIMULib/RTIMULib/RTFusion.h b/thirdparty/RTIMULib/RTIMULib/RTFusion.h
new file mode 100644
index 0000000..8f0cbbc
--- /dev/null
+++ b/thirdparty/RTIMULib/RTIMULib/RTFusion.h
@@ -0,0 +1,105 @@
+////////////////////////////////////////////////////////////////////////////
+//
+// 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 _RTFUSION_H
+#define _RTFUSION_H
+
+#include "RTIMULibDefs.h"
+
+class RTIMUSettings;
+
+class RTFusion
+{
+public:
+
+ RTFusion();
+ virtual ~RTFusion();
+
+ // fusionType returns the type code of the fusion algorithm
+
+ virtual int fusionType() { return RTFUSION_TYPE_NULL; }
+
+ // the following function can be called to set the SLERP power
+
+ void setSlerpPower(RTFLOAT power) { m_slerpPower = power; }
+
+ // reset() resets the fusion state but keeps any setting changes (such as enables)
+
+ virtual void reset() {}
+
+ // newIMUData() should be called for subsequent updates
+ // the fusion fields are updated with the results
+
+ virtual void newIMUData(RTIMU_DATA& /* data */, const RTIMUSettings * /* settings */) {}
+
+ // This static function returns performs the type to name mapping
+
+ static const char *fusionName(int fusionType) { return m_fusionNameMap[fusionType]; }
+
+ // the following three functions control the influence of the gyro, accel and compass sensors
+
+ void setGyroEnable(bool enable) { m_enableGyro = enable;}
+ void setAccelEnable(bool enable) { m_enableAccel = enable; }
+ void setCompassEnable(bool enable) { m_enableCompass = enable;}
+
+ inline const RTVector3& getMeasuredPose() {return m_measuredPose;}
+ inline const RTQuaternion& getMeasuredQPose() {return m_measuredQPose;}
+
+ // getAccelResiduals() gets the residual after subtracting gravity
+
+ RTVector3 getAccelResiduals();
+
+ void setDebugEnable(bool enable) { m_debug = enable; }
+
+protected:
+ void calculatePose(const RTVector3& accel, const RTVector3& mag, float magDeclination); // generates pose from accels and mag
+
+ RTVector3 m_gyro; // current gyro sample
+ RTVector3 m_accel; // current accel sample
+ RTVector3 m_compass; // current compass sample
+
+ RTQuaternion m_measuredQPose; // quaternion form of pose from measurement
+ RTVector3 m_measuredPose; // vector form of pose from measurement
+ RTQuaternion m_fusionQPose; // quaternion form of pose from fusion
+ RTVector3 m_fusionPose; // vector form of pose from fusion
+
+ RTQuaternion m_gravity; // the gravity vector as a quaternion
+
+ RTFLOAT m_slerpPower; // a value 0 to 1 that controls measured state influence
+ RTQuaternion m_rotationDelta; // amount by which measured state differs from predicted
+ RTQuaternion m_rotationPower; // delta raised to the appopriate power
+ RTVector3 m_rotationUnitVector; // the vector part of the rotation delta
+
+ bool m_debug;
+ bool m_enableGyro; // enables gyro as input
+ bool m_enableAccel; // enables accel as input
+ bool m_enableCompass; // enables compass a input
+ bool m_compassValid; // true if compass data valid
+
+ bool m_firstTime; // if first time after reset
+ uint64_t m_lastFusionTime; // for delta time calculation
+
+ static const char *m_fusionNameMap[]; // the fusion name array
+};
+
+#endif // _RTFUSION_H