115 lines
5.0 KiB
C
115 lines
5.0 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.
|
||
|
|
|
||
|
|
#ifndef _RTIMU_H
|
||
|
|
#define _RTIMU_H
|
||
|
|
|
||
|
|
#include "RTMath.h"
|
||
|
|
#include "RTIMULibDefs.h"
|
||
|
|
#include "I2Cdev.h"
|
||
|
|
|
||
|
|
#define I2CWrite(x, y, z) I2Cdev::writeByte(x, y, z)
|
||
|
|
#define I2CRead(w, x, y, z) I2Cdev::readBytes(w, x, y, z)
|
||
|
|
|
||
|
|
class RTIMUSettings;
|
||
|
|
|
||
|
|
class RTIMU
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
// IMUs should always be created with the following call
|
||
|
|
|
||
|
|
static RTIMU *createIMU(RTIMUSettings *settings);
|
||
|
|
|
||
|
|
// Constructor/destructor
|
||
|
|
|
||
|
|
RTIMU(RTIMUSettings *settings);
|
||
|
|
virtual ~RTIMU();
|
||
|
|
|
||
|
|
// These functions must be provided by sub classes
|
||
|
|
|
||
|
|
virtual const char *IMUName() = 0; // the name of the IMU
|
||
|
|
virtual int IMUType() = 0; // the type code of the IMU
|
||
|
|
virtual int IMUInit() = 0; // set up the IMU
|
||
|
|
virtual int IMUGetPollInterval() = 0; // returns the recommended poll interval in mS
|
||
|
|
virtual bool IMURead() = 0; // get a sample
|
||
|
|
|
||
|
|
// This one wanted a similar name but isn't pure virtual
|
||
|
|
|
||
|
|
virtual bool IMUCompassCalValid() { return m_calibrationValid; }
|
||
|
|
|
||
|
|
// setCalibrationMode() turns off use of cal data so that raw data can be accumulated
|
||
|
|
// to derive calibration data
|
||
|
|
|
||
|
|
void setCalibrationMode(bool enable) { m_calibrationMode = enable; }
|
||
|
|
|
||
|
|
// setCalibrationData configured the cal data and also enables use if valid
|
||
|
|
|
||
|
|
void setCalibrationData();
|
||
|
|
|
||
|
|
// getCalibrationValid() returns true if the calibration data is being used
|
||
|
|
|
||
|
|
bool getCalibrationValid() { return !m_calibrationMode && m_calibrationValid; }
|
||
|
|
|
||
|
|
// returns true if enough samples for valid data
|
||
|
|
|
||
|
|
virtual bool IMUGyroBiasValid();
|
||
|
|
|
||
|
|
inline const RTVector3& getGyro() { return m_gyro; } // gets gyro rates in radians/sec
|
||
|
|
inline const RTVector3& getAccel() { return m_accel; } // get accel data in gs
|
||
|
|
inline const RTVector3& getCompass() { return m_compass; } // gets compass data in uT
|
||
|
|
inline unsigned long getTimestamp() { return m_timestamp; } // and the timestamp for it
|
||
|
|
|
||
|
|
protected:
|
||
|
|
void gyroBiasInit(); // sets up gyro bias calculation
|
||
|
|
void handleGyroBias(); // adjust gyro for bias
|
||
|
|
void calibrateAverageCompass(); // calibrate and smooth compass
|
||
|
|
bool m_calibrationMode; // true if cal mode so don't use cal data!
|
||
|
|
bool m_calibrationValid; // tru if call data is valid and can be used
|
||
|
|
|
||
|
|
RTVector3 m_gyro; // the gyro readings
|
||
|
|
RTVector3 m_accel; // the accel readings
|
||
|
|
RTVector3 m_compass; // the compass readings
|
||
|
|
unsigned long m_timestamp; // the timestamp
|
||
|
|
|
||
|
|
RTIMUSettings *m_settings; // the settings object pointer
|
||
|
|
|
||
|
|
int m_sampleRate; // samples per second
|
||
|
|
uint64_t m_sampleInterval; // interval between samples in microseonds
|
||
|
|
|
||
|
|
RTFLOAT m_gyroAlpha; // gyro bias learning rate
|
||
|
|
int m_gyroSampleCount; // number of gyro samples used
|
||
|
|
bool m_gyroBiasValid; // true if the recorded gyro bias is valid
|
||
|
|
RTVector3 m_gyroBias; // the recorded gyro bias
|
||
|
|
|
||
|
|
RTVector3 m_previousAccel; // previous step accel for gyro learning
|
||
|
|
|
||
|
|
RTFLOAT m_compassCalOffset[3];
|
||
|
|
RTFLOAT m_compassCalScale[3];
|
||
|
|
RTVector3 m_compassAverage; // a running average to smooth the mag outputs
|
||
|
|
|
||
|
|
static RTFLOAT m_axisRotation[9]; // axis rotation matrix
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif // _RTIMU_H
|