abstract_imu.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * Copyright (c) 2020-2025 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_ABSTRACT_IMU_HPP_
#define TAPROOT_ABSTRACT_IMU_HPP_
 
#include "tap/algorithms/MahonyAHRS.h"
#include "tap/algorithms/transforms/orientation.hpp"
#include "tap/algorithms/transforms/transform.hpp"
#include "tap/algorithms/transforms/vector.hpp"
#include "tap/architecture/periodic_timer.hpp"
#include "tap/communication/sensors/imu/imu_interface.hpp"
#include "tap/util_macros.hpp"
 
namespace tap::communication::sensors::imu
{
using tap::algorithms::transforms::Orientation;
using tap::algorithms::transforms::Transform;
 
constexpr float GRAVITY_MPS2 = 9.81f;
class AbstractIMU : public ImuInterface
{
public:
    AbstractIMU(const Transform& mountingTransform = Transform::identity())
        : mountingTransform(mountingTransform)
    {
    }
 
    void setMountingTransform(const Transform& transform);
 
    virtual ~AbstractIMU() = default;
 
    virtual void initialize(float sampleFrequency, float mahonyKp, float mahonyKi);
 
    virtual void requestCalibration();
 
    virtual void periodicIMUUpdate();
 
    virtual ImuState getImuState() const { return imuState; }
 
    mockable inline float getAx() const override { return imuData.accG.x(); }
    mockable inline float getAy() const override { return imuData.accG.y(); }
    mockable inline float getAz() const override { return imuData.accG.z(); }
    mockable inline float getAzMinusG() const { return imuData.accG.z() - GRAVITY_MPS2; }
 
    mockable inline float getGx() const override { return imuData.gyroRadPerSec.x(); }
    mockable inline float getGy() const override { return imuData.gyroRadPerSec.y(); }
    mockable inline float getGz() const override { return imuData.gyroRadPerSec.z(); }
 
    mockable inline float getTemp() const { return imuData.temperature; }
 
    virtual inline float getYaw() const override { return mahonyAlgorithm.getYaw(); }
    virtual inline float getPitch() const override { return mahonyAlgorithm.getPitch(); }
    virtual inline float getRoll() const override { return mahonyAlgorithm.getRoll(); }
 
    struct ImuData
    {
        tap::algorithms::transforms::Vector accRaw = {0, 0, 0};
        tap::algorithms::transforms::Vector gyroRaw = {0, 0, 0};
        tap::algorithms::transforms::Vector accOffsetRaw = {0, 0, 0};
        tap::algorithms::transforms::Vector gyroOffsetRaw = {0, 0, 0};
        tap::algorithms::transforms::Vector accG = {0, 0, 0};
        tap::algorithms::transforms::Vector gyroRadPerSec = {0, 0, 0};
 
        float temperature = 0;
    };
 
    void setCalibrationSamples(int sampleCount) { offsetSampleCount = sampleCount; }
 
protected:
    void resetOffsets();
    void computeOffsets();
    void setAccelOffset(float x, float y, float z);
    void setGyroOffset(float x, float y, float z);
 
    inline void applyTransform(ImuData& data)
    {
        data.accG = mountingTransform.apply(data.accG);
        data.gyroRadPerSec = mountingTransform.apply(data.gyroRadPerSec);
    }
 
    virtual inline float getAccelerationSensitivity() const = 0;
 
    tap::algorithms::transforms::Transform mountingTransform;
 
    Mahony mahonyAlgorithm;
 
    ImuState imuState = ImuState::IMU_NOT_CONNECTED;
    int calibrationSample = 0;
    int offsetSampleCount = 1000;
 
    ImuData imuData;
 
    tap::arch::PeriodicMicroTimer readTimeout;
 
    uint32_t prevIMUDataReceivedTime = 0;
};
 
}  // namespace tap::communication::sensors::imu
 
#endif  // TAPROOT_ABSTRACT_IMU_HPP_