ahrs_imu.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_IMU_AHRS_IMU_HPP
#define M5_UNIT_IMU_IMU_AHRS_IMU_HPP
 
#include <cstdint>
 
#include "ahrs_controller.hpp"
 
namespace m5 {
namespace imu {
 
class AhrsImu : public AhrsController {
public:
    struct imu_3d_t {
        union {
            float value[3];
            struct {
                float x;
                float y;
                float z;
            };
        };
    };
 
    struct imu_data_t {
        uint32_t usec;  
        union {
            float value[9];
            imu_3d_t sensor[3];
            struct {
                imu_3d_t accel;
                imu_3d_t gyro;
                imu_3d_t mag;
            };
        };
    };
 
    enum axis_t {
        axis_x_pos = 0,  
        axis_x_neg = 1,  
        axis_y_pos = 2,  
        axis_y_neg = 3,  
        axis_z_pos = 4,  
        axis_z_neg = 5,  
    };
 
    inline bool getAccel(float* const ax, float* const ay, float* const az) const
    {
        const imu_physical_data_t& s = latest();
        if (ax != nullptr) {
            *ax = s.accel[0];
        }
        if (ay != nullptr) {
            *ay = s.accel[1];
        }
        if (az != nullptr) {
            *az = s.accel[2];
        }
        return hasAccel();
    }
 
    inline bool getGyro(float* const gx, float* const gy, float* const gz) const
    {
        const imu_physical_data_t& s = latest();
        if (gx != nullptr) {
            *gx = s.gyro[0];
        }
        if (gy != nullptr) {
            *gy = s.gyro[1];
        }
        if (gz != nullptr) {
            *gz = s.gyro[2];
        }
        return hasGyro();
    }
 
    inline bool getMag(float* const mx, float* const my, float* const mz) const
    {
        const imu_physical_data_t& s = latest();
        if (mx != nullptr) {
            *mx = s.mag[0];
        }
        if (my != nullptr) {
            *my = s.mag[1];
        }
        if (mz != nullptr) {
            *mz = s.mag[2];
        }
        return hasValidMag();
    }
 
    inline bool getTemp(float* const t) const
    {
        if (t != nullptr) {
            *t = latest().temp;
        }
        return hasTemp();
    }
 
    inline void getImuData(imu_data_t* const data) const
    {
        if (data == nullptr) {
            return;
        }
        const imu_physical_data_t& s = latest();
        data->usec                   = static_cast<uint32_t>(lastMicros());
        data->accel.x                = s.accel[0];
        data->accel.y                = s.accel[1];
        data->accel.z                = s.accel[2];
        data->gyro.x                 = s.gyro[0];
        data->gyro.y                 = s.gyro[1];
        data->gyro.z                 = s.gyro[2];
        data->mag.x                  = s.mag[0];
        data->mag.y                  = s.mag[1];
        data->mag.z                  = s.mag[2];
    }
 
    inline const imu_data_t& getImuData()
    {
        getImuData(&_compat_data);
        return _compat_data;
    }
 
    inline void getAhrsData(float* const pitch, float* const roll, float* const yaw) const
    {
        float roll_deg{}, pitch_deg{}, yaw_deg{};
        euler(roll_deg, pitch_deg, yaw_deg);
        if (pitch != nullptr) {
            *pitch = pitch_deg;
        }
        if (roll != nullptr) {
            *roll = roll_deg;
        }
        if (yaw != nullptr) {
            *yaw = yaw_deg;
        }
    }
 
    inline bool getAccelData(float* const ax, float* const ay, float* const az) const
    {
        return getAccel(ax, ay, az);
    }
 
    inline bool getGyroData(float* const gx, float* const gy, float* const gz) const
    {
        return getGyro(gx, gy, gz);
    }
 
    inline bool getGyroMag(float* const mx, float* const my, float* const mz) const
    {
        return getMag(mx, my, mz);
    }
 
    inline bool isEnabled() const
    {
        return hasAccel() && hasGyro();
    }
 
    inline void setCalibration(const uint8_t accel_strength, const uint8_t gyro_strength, const uint8_t mag_strength)
    {
        writeAutoCalibration(accel_strength, gyro_strength, mag_strength);
    }
 
    inline void clearOffsetData()
    {
        writeCalibration(calibration_data_t{});
    }
 
    inline bool setAxisOrder(const axis_t axis0, const axis_t axis1, const axis_t axis2)
    {
        const uint8_t idx[3] = {
            static_cast<uint8_t>(axis0 >> 1),
            static_cast<uint8_t>(axis1 >> 1),
            static_cast<uint8_t>(axis2 >> 1),
        };
        if (idx[0] > 2 || idx[1] > 2 || idx[2] > 2 || idx[0] == idx[1] || idx[1] == idx[2] || idx[0] == idx[2]) {
            return false;
        }
        AxisOrder order;
        order.permutation = permutation_from_indices(idx);
        order.invert_x    = (axis0 & 1) != 0;
        order.invert_y    = (axis1 & 1) != 0;
        order.invert_z    = (axis2 & 1) != 0;
        writeUserAxisOrder(order);
        return true;
    }
 
    inline bool setAxisOrderRightHanded(const axis_t axis0, const axis_t axis1)
    {
        return setAxisOrder(axis0, axis1, getAxis2(axis0, axis1));
    }
 
    inline bool setAxisOrderLeftHanded(const axis_t axis0, const axis_t axis1)
    {
        return setAxisOrder(axis0, axis1, static_cast<axis_t>(getAxis2(axis0, axis1) ^ 1));
    }
 
private:
    // Right-handed third axis for a given first two (ported from m5::IMU_Class getAxis2)
    static inline axis_t getAxis2(const axis_t axis0, const axis_t axis1)
    {
        uint8_t axis2     = static_cast<uint8_t>(((1 << (axis0 >> 1)) | (1 << (axis1 >> 1))) ^ 0x07);
        axis2             = static_cast<uint8_t>(axis2 & 0x06);
        const uint8_t ax0 = axis2 ? static_cast<uint8_t>(axis2 - 2) : 4;
        axis2             = static_cast<uint8_t>(axis2 + (((axis0 & 0x06) == ax0) == ((axis0 & 1) == (axis1 & 1))));
        return static_cast<axis_t>(axis2);
    }
 
    imu_data_t _compat_data{};  // Scratch for the reference-returning getImuData() overload
};
 
}  // namespace imu
}  // namespace m5
#endif