c2-utopia/lib/RTIMULib/RTIMU.cpp

358 lines
10 KiB
C++

////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// 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 "RTIMU.h"
#include "RTIMUSettings.h"
#include "CalLib.h"
// this sets the learning rate for compass running average calculation
#define COMPASS_ALPHA (RTFLOAT)0.2
// this defines the accelerometer noise level
#define RTIMU_FUZZY_GYRO_ZERO (RTFLOAT)0.20
#define RTIMU_FUZZY_GYRO_ZERO_SQUARED (RTIMU_FUZZY_GYRO_ZERO * RTIMU_FUZZY_GYRO_ZERO)
// this defines the accelerometer noise level
#define RTIMU_FUZZY_ACCEL_ZERO (RTFLOAT)0.05
#define RTIMU_FUZZY_ACCEL_ZERO_SQUARED (RTIMU_FUZZY_ACCEL_ZERO * RTIMU_FUZZY_ACCEL_ZERO)
// Axis rotation array
#ifdef RTIMU_XNORTH_YEAST
RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
#endif
#ifdef RTIMU_XEAST_YSOUTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, -1, 0, 1, 0, 0, 0, 0, 1};
#endif
#ifdef RTIMU_XSOUTH_YWEST
RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, -1, 0, 0, 0, 1};
#endif
#ifdef RTIMU_XWEST_YNORTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, -1, 0, 0, 0, 0, 1};
#endif
#ifdef RTIMU_XNORTH_YWEST
RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
#endif
#ifdef RTIMU_XEAST_YNORTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, 1, 0, 0, 0, 0, -1};
#endif
#ifdef RTIMU_XSOUTH_YEAST
RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1};
#endif
#ifdef RTIMU_XWEST_YSOUTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, -1, 0, -1, 0, 0, 0, 0, -1};
#endif
#ifdef RTIMU_XUP_YNORTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, 0, 0, -1, -1, 0, 0};
#endif
#ifdef RTIMU_XUP_YEAST
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, 0, 1, 0, -1, 0, 0};
#endif
#ifdef RTIMU_XUP_YSOUTH
{0, -1, 0, 0, 0, 1, -1, 0, 0};
#endif
#ifdef RTIMU_XUP_YWEST
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, 0, -1, 0, -1, 0, 0};
#endif
#ifdef RTIMU_XDOWN_YNORTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, 0, 0, 1, 1, 0, 0};
#endif
#ifdef RTIMU_XDOWN_YEAST
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, 0, 1, 0, 1, 0, 0};
#endif
#ifdef RTIMU_XDOWN_YSOUTH
RTFLOAT RTIMU::m_axisRotation[9] = {0, -1, 0, 0, 0, -1, 1, 0, 0};
#endif
#ifdef RTIMU_XDOWN_YWEST
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, 0, -1, 0, 1, 0, 0};
#endif
#ifdef RTIMU_XNORTH_YUP
RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, 0, 1, 0, -1, 0};
#endif
#ifdef RTIMU_XEAST_YUP
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, 1, 0, 0, 0, -1, 0};
#endif
#ifdef RTIMU_XSOUTH_YUP
RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, 0, -1, 0, -1, 0};
#endif
#ifdef RTIMU_XWEST_YUP
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, -1, 0, 0, 0, -1, 0};
#endif
#ifdef RTIMU_XNORTH_YDOWN
RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
#endif
#ifdef RTIMU_XEAST_YDOWN
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, 1, 0, 0, 0, 1, 0};
#endif
#ifdef RTIMU_XSOUTH_YDOWN
RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, 0, 1, 0, 1, 0};
#endif
#ifdef RTIMU_XWEST_YDOWN
RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, -1, 0, 0, 0, 1, 0};
#endif
#if defined(MPU9150_68) || defined(MPU9150_69)
#include "RTIMUMPU9150.h"
#endif
#if defined(MPU9250_68) || defined(MPU9250_69)
#include "RTIMUMPU9250.h"
#endif
#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
#include "RTIMULSM9DS0.h"
#endif
#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
#include "RTIMUGD20HM303D.h"
#endif
#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
#include "RTIMUGD20M303DLHC.h"
#endif
#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
#include "RTIMUGD20HM303DLHC.h"
#endif
#if defined(BNO055_28) || defined(BNO055_29)
#include "RTIMUBNO055.h"
#endif
RTIMU *RTIMU::createIMU(RTIMUSettings *settings)
{
#if defined(MPU9150_68) || defined(MPU9150_69)
return new RTIMUMPU9150(settings);
#endif
#if defined(MPU9250_68) || defined(MPU9250_69)
return new RTIMUMPU9250(settings);
#endif
#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
return new RTIMULSM9DS0(settings);
#endif
#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
return new RTIMUGD20HM303D(settings);
#endif
#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
return new RTIMUGD20M303DLHC(settings);
#endif
#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
return new RTIMUGD20HM303DLHC(settings);
#endif
#if defined(BNO055_28) || defined(BNO055_29)
return new RTIMUBNO055(settings);
#endif
}
RTIMU::RTIMU(RTIMUSettings *settings)
{
m_settings = settings;
m_calibrationMode = false;
m_calibrationValid = false;
m_gyroBiasValid = false;
}
RTIMU::~RTIMU()
{
}
void RTIMU::setCalibrationData()
{
float maxDelta = -1;
float delta;
CALLIB_DATA calData;
m_calibrationValid = false;
if (calLibRead(0, &calData)) {
if (calData.magValid != 1) {
return;
}
// find biggest range
for (int i = 0; i < 3; i++) {
if ((calData.magMax[i] - calData.magMin[i]) > maxDelta)
maxDelta = calData.magMax[i] - calData.magMin[i];
}
if (maxDelta < 0) {
return;
}
maxDelta /= 2.0f; // this is the max +/- range
for (int i = 0; i < 3; i++) {
delta = (calData.magMax[i] - calData.magMin[i]) / 2.0f;
m_compassCalScale[i] = maxDelta / delta; // makes everything the same range
m_compassCalOffset[i] = (calData.magMax[i] + calData.magMin[i]) / 2.0f;
}
m_calibrationValid = true;
}
}
void RTIMU::gyroBiasInit()
{
m_gyroAlpha = 2.0f / m_sampleRate;
m_gyroSampleCount = 0;
}
// Note - code assumes that this is the first thing called after axis swapping
// for each specific IMU chip has occurred.
void RTIMU::handleGyroBias()
{
// do axis rotation if necessary
#ifndef RTIMU_XNORTH_YEAST
// need to do an axis rotation
float *matrix = m_axisRotation;
RTVector3 tempGyro = m_gyro;
RTVector3 tempAccel = m_accel;
RTVector3 tempCompass = m_compass;
// do new x value
if (matrix[0] != 0) {
m_gyro.setX(tempGyro.x() * matrix[0]);
m_accel.setX(tempAccel.x() * matrix[0]);
m_compass.setX(tempCompass.x() * matrix[0]);
} else if (matrix[1] != 0) {
m_gyro.setX(tempGyro.y() * matrix[1]);
m_accel.setX(tempAccel.y() * matrix[1]);
m_compass.setX(tempCompass.y() * matrix[1]);
} else if (matrix[2] != 0) {
m_gyro.setX(tempGyro.z() * matrix[2]);
m_accel.setX(tempAccel.z() * matrix[2]);
m_compass.setX(tempCompass.z() * matrix[2]);
}
// do new y value
if (matrix[3] != 0) {
m_gyro.setY(tempGyro.x() * matrix[3]);
m_accel.setY(tempAccel.x() * matrix[3]);
m_compass.setY(tempCompass.x() * matrix[3]);
} else if (matrix[4] != 0) {
m_gyro.setY(tempGyro.y() * matrix[4]);
m_accel.setY(tempAccel.y() * matrix[4]);
m_compass.setY(tempCompass.y() * matrix[4]);
} else if (matrix[5] != 0) {
m_gyro.setY(tempGyro.z() * matrix[5]);
m_accel.setY(tempAccel.z() * matrix[5]);
m_compass.setY(tempCompass.z() * matrix[5]);
}
// do new z value
if (matrix[6] != 0) {
m_gyro.setZ(tempGyro.x() * matrix[6]);
m_accel.setZ(tempAccel.x() * matrix[6]);
m_compass.setZ(tempCompass.x() * matrix[6]);
} else if (matrix[7] != 0) {
m_gyro.setZ(tempGyro.y() * matrix[7]);
m_accel.setZ(tempAccel.y() * matrix[7]);
m_compass.setZ(tempCompass.y() * matrix[7]);
} else if (matrix[8] != 0) {
m_gyro.setZ(tempGyro.z() * matrix[8]);
m_accel.setZ(tempAccel.z() * matrix[8]);
m_compass.setZ(tempCompass.z() * matrix[8]);
}
#endif
if (!m_gyroBiasValid) {
RTVector3 deltaAccel = m_previousAccel;
deltaAccel -= m_accel; // compute difference
m_previousAccel = m_accel;
if ((deltaAccel.squareLength() < RTIMU_FUZZY_ACCEL_ZERO_SQUARED) &&
(m_gyro.squareLength() < RTIMU_FUZZY_GYRO_ZERO_SQUARED)) {
// what we are seeing on the gyros should be bias only so learn from this
m_gyroBias.setX((1.0 - m_gyroAlpha) * m_gyroBias.x() + m_gyroAlpha * m_gyro.x());
m_gyroBias.setY((1.0 - m_gyroAlpha) * m_gyroBias.y() + m_gyroAlpha * m_gyro.y());
m_gyroBias.setZ((1.0 - m_gyroAlpha) * m_gyroBias.z() + m_gyroAlpha * m_gyro.z());
if (m_gyroSampleCount < (5 * m_sampleRate)) {
m_gyroSampleCount++;
if (m_gyroSampleCount == (5 * m_sampleRate)) {
m_gyroBiasValid = true;
}
}
}
}
m_gyro -= m_gyroBias;
}
void RTIMU::calibrateAverageCompass()
{
// calibrate if required
if (!m_calibrationMode && m_calibrationValid) {
m_compass.setX((m_compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
m_compass.setY((m_compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
m_compass.setZ((m_compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
}
// update running average
m_compassAverage.setX(m_compass.x() * COMPASS_ALPHA + m_compassAverage.x() * (1.0 - COMPASS_ALPHA));
m_compassAverage.setY(m_compass.y() * COMPASS_ALPHA + m_compassAverage.y() * (1.0 - COMPASS_ALPHA));
m_compassAverage.setZ(m_compass.z() * COMPASS_ALPHA + m_compassAverage.z() * (1.0 - COMPASS_ALPHA));
m_compass = m_compassAverage;
}
bool RTIMU::IMUGyroBiasValid()
{
return m_gyroBiasValid;
}