Program Listing for File bmi088.hpp

↰ Return to documentation for file (src/tap/communication/sensors/imu/bmi088/bmi088.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_BMI088_HPP_
#define TAPROOT_BMI088_HPP_

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

#include "bmi088_data.hpp"

namespace tap
{
class Drivers;
}

namespace tap::communication::sensors::imu::bmi088
{
class Bmi088 final_mockable : public Bmi088Data, public ImuInterface
{
public:
    static constexpr Acc::AccRange_t ACC_RANGE = Acc::AccRange::G3;
    static constexpr Gyro::GyroRange_t GYRO_RANGE = Gyro::GyroRange::DPS2000;
    static constexpr float GYRO_RANGE_MAX_DS = 2000.0f;

    static constexpr float BMI088_TEMP_FACTOR = 0.125f;
    static constexpr float BMI088_TEMP_OFFSET = 23.0f;

    static constexpr float GYRO_DS_PER_GYRO_COUNT = GYRO_RANGE_MAX_DS / 32767.0f;

    static constexpr float ACC_G_PER_ACC_COUNT =
        modm::pow(2, ACC_RANGE.value + 1) * 1.5f * tap::algorithms::ACCELERATION_GRAVITY / 32768.0f;

    float BMI088_OFFSET_SAMPLES = 1000;

    Bmi088(tap::Drivers *drivers);

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

    mockable void periodicIMUUpdate();

    mockable void read();

    mockable ImuState getImuState() const;

    mockable void requestRecalibration();

    inline const char *getName() const final_mockable { return "bmi088"; }

    mockable inline float getYaw() final_mockable { return mahonyAlgorithm.getYaw(); }
    mockable inline float getPitch() final_mockable { return mahonyAlgorithm.getPitch(); }
    mockable inline float getRoll() final_mockable { return mahonyAlgorithm.getRoll(); }

    mockable inline float getGx() final_mockable { return data.gyroDegPerSec[ImuData::X]; }
    mockable inline float getGy() final_mockable { return data.gyroDegPerSec[ImuData::Y]; }
    mockable inline float getGz() final_mockable { return data.gyroDegPerSec[ImuData::Z]; }

    mockable inline float getAx() final_mockable { return data.accG[ImuData::X]; }
    mockable inline float getAy() final_mockable { return data.accG[ImuData::Y]; }
    mockable inline float getAz() final_mockable { return data.accG[ImuData::Z]; }

    mockable inline float getTemp() final_mockable { return data.temperature; }

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

    inline void setOffsetSamples(float samples) { BMI088_OFFSET_SAMPLES = samples; }

    inline void setAccOversampling(Acc::AccBandwidth oversampling)
    {
        accOversampling = oversampling;
    }
    inline void setAccOutputRate(Acc::AccOutputRate outputRate) { accOutputRate = outputRate; }

    inline void setGyroOutputRate(Gyro::GyroBandwidth outputRate) { gyroOutputRate = outputRate; }

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

private:
    static constexpr uint16_t RAW_TEMPERATURE_TO_APPLY_OFFSET = 1023;
    static constexpr int16_t RAW_TEMPERATURE_OFFSET = -2048;

    struct ImuData
    {
        enum Axis
        {
            X = 0,
            Y = 1,
            Z = 2,
        };

        float accRaw[3] = {};
        float gyroRaw[3] = {};
        float accOffsetRaw[3] = {};
        float gyroOffsetRaw[3] = {};
        float accG[3] = {};
        float gyroDegPerSec[3] = {};

        float temperature;
    } data;

    tap::Drivers *drivers;

    ImuState imuState = ImuState::IMU_NOT_CONNECTED;

    Mahony mahonyAlgorithm;

    imu_heater::ImuHeater imuHeater;

    int calibrationSample = 0;

    uint32_t prevIMUDataReceivedTime = 0;

    Acc::AccBandwidth accOversampling = Acc::AccBandwidth::NORMAL;
    Acc::AccOutputRate accOutputRate = Acc::AccOutputRate::Hz800;

    void initializeAcc();

    Gyro::GyroBandwidth gyroOutputRate = Gyro::GyroBandwidth::ODR1000_BANDWIDTH116;
    void initializeGyro();

    void computeOffsets();

    void setAndCheckAccRegister(Acc::Register reg, Acc::Registers_t value);

    void setAndCheckGyroRegister(Gyro::Register reg, Gyro::Registers_t value);

    static inline float parseTemp(uint8_t tempMsb, uint8_t tempLsb)
    {
        uint16_t temp =
            (static_cast<uint16_t>(tempMsb) * 8) + (static_cast<uint16_t>(tempLsb) / 32);

        int16_t shiftedTemp = static_cast<int16_t>(temp);

        if (temp > RAW_TEMPERATURE_TO_APPLY_OFFSET)
        {
            shiftedTemp += RAW_TEMPERATURE_OFFSET;
        }

        return static_cast<float>(shiftedTemp) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
    }
};

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

#endif  // TAPROOT_BMI088_HPP_