Program Listing for File mpu6500.hpp

↰ Return to documentation for file (src/tap/communication/sensors/imu/mpu6500/mpu6500.hpp)

/*
 * Copyright (c) 2020-2022 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef TAPROOT_MPU6500_HPP_
#define TAPROOT_MPU6500_HPP_

#include <cstdint>

#include "tap/algorithms/MahonyAHRS.h"
#include "tap/architecture/timeout.hpp"
#include "tap/communication/sensors/imu/imu_interface.hpp"
#include "tap/communication/sensors/imu_heater/imu_heater.hpp"
#include "tap/util_macros.hpp"

#include "modm/math/geometry.hpp"
#include "modm/processing/protothread.hpp"

#define LITTLE_ENDIAN_INT16_TO_FLOAT(buff) \
    (static_cast<float>(static_cast<int16_t>((*(buff) << 8) | *(buff + 1))))

namespace tap
{
class Drivers;
}

namespace tap::communication::sensors::imu::mpu6500
{
class Mpu6500 final_mockable : public ::modm::pt::Protothread, public ImuInterface
{
public:
    static constexpr uint8_t ACC_GYRO_TEMPERATURE_BUFF_RX_SIZE = 14;

    struct RawData
    {
        modm::Vector3f accel;
        modm::Vector3f gyro;

        uint16_t temperature = 0;

        modm::Vector3f accelOffset;
        modm::Vector3f gyroOffset;
    };

    using ProcessRawMpu6500DataFn = void (*)(
        const uint8_t (&)[ACC_GYRO_TEMPERATURE_BUFF_RX_SIZE],
        modm::Vector3f &accel,
        modm::Vector3f &gyro);

    Mpu6500(Drivers *drivers);
    DISALLOW_COPY_AND_ASSIGN(Mpu6500)
    mockable ~Mpu6500() = default;

    mockable void init(float sampleFrequency, float mahonyKp, float mahonyKi);

    mockable void periodicIMUUpdate();

    mockable bool read();

    bool run() { return this->read(); }

    mockable inline ImuState getImuState() const { return imuState; }

    virtual inline const char *getName() const { return "mpu6500"; }

    inline float validateReading(float reading)
    {
        if (imuState == ImuState::IMU_CALIBRATED)
        {
            return reading;
        }
        else if (imuState == ImuState::IMU_NOT_CALIBRATED)
        {
            errorState |= 1 << static_cast<uint8_t>(ImuState::IMU_NOT_CALIBRATED);
            return reading;
        }
        else if (imuState == ImuState::IMU_CALIBRATING)
        {
            errorState |= 1 << static_cast<uint8_t>(ImuState::IMU_CALIBRATING);
            return 0.0f;
        }
        else
        {
            errorState |= 1 << static_cast<uint8_t>(ImuState::IMU_NOT_CONNECTED);
            return 0.0f;
        }
    }

    inline float getAx() final_mockable
    {
        return validateReading(
            static_cast<float>(raw.accel.x - raw.accelOffset.x) * ACCELERATION_GRAVITY /
            ACCELERATION_SENSITIVITY);
    }

    inline float getAy() final_mockable
    {
        return validateReading(
            static_cast<float>(raw.accel.y - raw.accelOffset.y) * ACCELERATION_GRAVITY /
            ACCELERATION_SENSITIVITY);
    }

    inline float getAz() final_mockable
    {
        return validateReading(
            static_cast<float>(raw.accel.z - raw.accelOffset.z) * ACCELERATION_GRAVITY /
            ACCELERATION_SENSITIVITY);
    }

    inline float getGx() final_mockable
    {
        return validateReading(
            static_cast<float>(raw.gyro.x - raw.gyroOffset.x) / LSB_D_PER_S_TO_D_PER_S);
    }

    inline float getGy() final_mockable
    {
        return validateReading(
            static_cast<float>(raw.gyro.y - raw.gyroOffset.y) / LSB_D_PER_S_TO_D_PER_S);
    }

    inline float getGz() final_mockable
    {
        return validateReading(
            static_cast<float>(raw.gyro.z - raw.gyroOffset.z) / LSB_D_PER_S_TO_D_PER_S);
    }

    inline float getTemp() final_mockable
    {
        return 21.0f + static_cast<float>(raw.temperature) / 333.87f;
    }

    inline float getYaw() final_mockable { return validateReading(mahonyAlgorithm.getYaw()); }

    inline float getPitch() final_mockable { return validateReading(mahonyAlgorithm.getPitch()); }

    inline float getRoll() final_mockable { return validateReading(mahonyAlgorithm.getRoll()); }

    mockable inline uint32_t getPrevIMUDataReceivedTime() const { return prevIMUDataReceivedTime; }

    mockable float getTiltAngle();

    mockable void requestCalibration();

    void attachProcessRawMpu6500DataFn(ProcessRawMpu6500DataFn fn) { processRawMpu6500DataFn = fn; }

    static constexpr float LSB_D_PER_S_TO_D_PER_S = 16.384f;

    inline void setCalibrationSamples(float samples) { MPU6500_OFFSET_SAMPLES = samples; }

    inline void setTargetTemperature(float temperatureC)
    {
        imuHeater.setDesiredTemperature(temperatureC);
    }

private:
    static constexpr float ACCELERATION_GRAVITY = 9.80665f;

    static constexpr float ACCELERATION_SENSITIVITY = 4096.0f;

    float MPU6500_OFFSET_SAMPLES = 1000;

    static constexpr int NONBLOCKING_TIME_TO_READ_REG = 450;

    static constexpr uint32_t MAX_WAIT_FOR_IMU_TEMPERATURE_STABALIZE = 10'000;

    static constexpr uint32_t WAIT_TIME_AFTER_CALIBRATION = 10'000;

    static constexpr uint8_t MPU6500_READ_BIT = 0x80;

    Drivers *drivers;

    ProcessRawMpu6500DataFn processRawMpu6500DataFn;

    int delayBtwnCalcAndReadReg = 2000 - NONBLOCKING_TIME_TO_READ_REG;

    ImuState imuState = ImuState::IMU_NOT_CONNECTED;

    tap::arch::MicroTimeout readRegistersTimeout;
    uint8_t tx = 0;
    uint8_t rx = 0;

    RawData raw;

    Mahony mahonyAlgorithm;

    imu_heater::ImuHeater imuHeater;

    float tiltAngle = 0.0f;
    bool tiltAngleCalculated = false;

    uint8_t txBuff[ACC_GYRO_TEMPERATURE_BUFF_RX_SIZE] = {0};

    uint8_t rxBuff[ACC_GYRO_TEMPERATURE_BUFF_RX_SIZE] = {0};

    int calibrationSample = 0;

    uint8_t errorState = 0;

    uint32_t prevIMUDataReceivedTime = 0;

    // Functions for interacting with hardware directly.

    void mpuNssLow();

    void mpuNssHigh();

    void spiWriteRegister(uint8_t reg, uint8_t data);

    uint8_t spiReadRegister(uint8_t reg);

    void spiReadRegisters(uint8_t regAddr, uint8_t *pData, uint8_t len);

    void addValidationErrors();

    static void defaultProcessRawMpu6500Data(
        const uint8_t (&rxBuff)[ACC_GYRO_TEMPERATURE_BUFF_RX_SIZE],
        modm::Vector3f &accel,
        modm::Vector3f &gyro);
};

}  // namespace tap::communication::sensors::imu::mpu6500

#endif  // TAPROOT_MPU6500_HPP_