358 lines
10 KiB
C++
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;
|
|
}
|
|
|
|
|