ISM330.h

Go to the documentation of this file.

 
#pragma once
 
#ifndef ISM330_H_
#define ISM330_H_
#include "mbed.h"
#include "util/peripherals/imu/IMU.h"
#include <cstdint>
 
class ISM330 : public IMU {
public:
    // Structs
    typedef struct {
        float x;
        float y;
        float z;
    } ISM330_VECTOR_TypeDef;
 
    typedef struct {
        int16_t ax;
        int16_t ay;
        int16_t az;
        int16_t gx;
        int16_t gy;
        int16_t gz;
    } ISM330_RAW_DATA_TypeDef;
 
    typedef struct {
        float ax;
        float ay;
        float az;
        float gx;
        float gy;
        float gz;
    } ISM330_DATA_TypeDef;
 
 
    ISM330(I2C &i2c, uint8_t address) noexcept;
 
    bool begin(float prop_gain=0.1f, float int_gain=0.0f) noexcept;
 
    uint8_t whoAmI() noexcept;
    
    void reset() noexcept;
    
    void calibrate() noexcept; 
    
    IMU::EulerAngles getImuAngles() override;
    
    IMU::EulerAngles read() override { return getImuAngles(); }
 
   //Full read functions
    //Reads Accel and Gyro sequentially, reduces I2C transactions
    ISM330_RAW_DATA_TypeDef readAGraw() noexcept; //used during calibration
 
    ISM330_DATA_TypeDef readAG() noexcept;
 
 
    // Temperature Reading
    float readTemperature() noexcept; // Returns temp in C-25, so at 25C it should read 0
    // If you're wondering why, it's just how the IMU is calibrated
 
    float accelTempScale(float temp); // Returns the scaling of the accelerometer with respect to Temperature; At 25C = 0;
 
    float accelTempOffset(float temp); // Returns the offset of the accelerometer with respect to Temperature; At 25C = 0; (Just adding a constant)
 
    float gyroTempScale(float temp); // Returns the scaling of the gyro with respect to Temperature; At 25C = 0;
 
    float gyroTempOffset(float temp); // Returns the offset of the gyro with respect to Temperature; At 25C = 0; (Just adding a constant)
 
    // Update Structs
    void getAGVectors(ISM330_VECTOR_TypeDef& accel, ISM330_VECTOR_TypeDef& gyro);
 
    // IMU Sensor Fusion
    void computeAnglesMah(IMU::EulerAngles& angles); // Mahony Compute Euler Angles
 
    void computeAnglesMadgwick(IMU::EulerAngles& angles);
    
    // Mahony Sensor Fusion
    //Adafruit_Mahony(float prop_gain, float int_gain);
    void mahonyStart(float prop_gain, float int_gain);
 
    void initQuaternionFromAccel();
    
    void mahonyUpdate(float mx, float my, float mz, float dt);
    
    void mahonyUpdateIMU(float dt);
 
    // Madgwick Sensor Fusion
 
    void madgwickStart(float gain);
    
    void madgwickUpdate(float mx, float my, float mz, float dt);
    
    void madgwickUpdateIMU(float dt);
 
    // Reading individual sensor values
    ISM330_VECTOR_TypeDef readAccel() noexcept;
 
    ISM330_VECTOR_TypeDef readGyro() noexcept;
    
private:
    I2C &i2c;
    uint8_t _address;
    uint8_t whoAmIReading;
 
    // Raw offsets for accel and gyro
    float axBias;
    float ayBias;
    float azBias;
    float wxBias;
    float wyBias;
    float wzBias;
    
    //Raw I2C readings to actual measurements
    uint8_t agReadings[12]; //Accel and Gyro Readings buffer
    uint8_t xReadings[6]; //Accel Readings buffer
    uint8_t gReadings[6]; //Gyro Readings buffer
    float temperature; //Temperature in C-25, so at 25C it should read 0
 
    ISM330_VECTOR_TypeDef readingToAccel(const uint8_t *readings, float temp);
    
    ISM330_VECTOR_TypeDef readingToGyro(const uint8_t *readings, float temp);
        
    float invSqrt(float x); //Inverse square root, used for normalizing vectors
    
    float beta; // algorithm gain
    float q0;
    float q1;
    float q2;
    float q3; // quaternion of sensor frame relative to auxiliary frame
    float invSampleFreq;
    float grav[3]; // gravity vector 
 
    //Mahony Quaternions
    float mahq0;
    float mahq1;
    float mahq2;
    float mahq3;
 
    float twoKp; // 2 * proportional gain (Kp)
    float twoKi; // 2 * integral gain (Ki)
    float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
 
    bool anglesComputed = false; // Flag for whether or not angles have been computed
};
 
#endif // ISM330_H_