File content as of revision 25:3dfd686ae315:

/**
 *  @file imu_driver.hpp
 *  @brief ImuDriver class and other data storage struct definition. The C language version
 *         can be found at <A HREF="https://github.com/osjacky430/imu_driver">this github website</A>
 */

#ifndef IMU_DRIVER_HPP_
#define IMU_DRIVER_HPP_

#include <mbed.h>
#include <cstdint>

typedef enum ImuExtiConfig {
    ImuExtiRcvNotUsed = 0b00,
    ImuExtiRcvNormalMsg = 0b01,
    ImuExtiRcvAhrsMsg = 0b10,
    ImuExtiRcvBothMsg = 0b11
} ImuExtiConfig;

/* this needs to be extended (IMO) */
inline ImuExtiConfig operator |(ImuExtiConfig const& left, ImuExtiConfig const& right) 
{
    return ImuExtiConfig(left | right);
}

/**
 *  @brief  ImuDriverStatus
 */
typedef enum ImuDriverStatus {
    ImuDriverStatusOK,
    ImuDriverStatusDataNotReady,
    ImuDriverStatusInvalidCall
} ImuDriverStatus;

typedef enum ImuDriverRegister {
    ReadAhrsBurstDataRegister = 0x3D,
    ReadBurstDataRegister = 0x3E,
    ReadExtBurstDataRegister = 0x3F
} ImuDriverRegister;

typedef struct AhrsRawData {
    std::uint16_t status;
    std::int16_t attitude[3];
    std::int16_t temperature;
} AhrsRawData;

/**
 *  @brief  AhrsProcessedData is data storage class fpr processed AHRS data
 *
 *          The data storage struct consists of:
 *
 *              - attitude: pitch, roll, yaw in deg, range from -180 ~ +180 deg
 */
typedef struct AhrsProcessedData {
    float attitude[3];
} AhrsProcessedData;

typedef struct ImuRawData {
    std::uint16_t status;
    std::int16_t rate[3];
    std::int16_t accel[3];
    std::int16_t temperature;
} ImuRawData;

/**
 *  @brief  ImuProcessedData is data storage class for processed imu data
 *
 *           This data storage class consists of:
 *
 *              - status: if status = 0x0010 (over range error), status = 0 (no error)
 *              - gyroscope: deg/s, range from -300 deg/s ~ 300 deg/s
 *              - accelerometer: g, range from -4.5g ~ 4.5g
 *              - temperature: Celcius
 */
typedef struct ImuProcessedData {
    std::uint16_t status;

    float rate[3];
    float accel[3];

    std::int16_t temperature;
} ImuProcessedData;

/**
 *  @brief ImuDriver is used to handle the master side of SPI communication of openIMU300ZI module, the
 *         default setups are:
 *
 *           - Data frame: 16 bits
 *           - CPOL: High (1)
 *           - CPHA: 2 Edge (1)
 *           - Frequency: 1 MHz (Default frequency of spi)
 *           - Endian: MSB first (Mbed only support MSB AFAIK)
 *
 *         According to <A HREF="https://openimu.readthedocs.io/en/latest/software/SPImessaging.html">openIMU300ZI SPI framework</A>:
 *
 *           - Data transferred in 16-bit word-length and MSB-first
 *           - fCLK ≤ 2.0 MHz
 *           - CPOL = 1 (clock polarity) and CPHA = 1 (clock phase)
 *
 *  @tparam spi     SPI comm instance, ImuDriver currently support only SPI framework
 *  @tparam rst     Reset pin name, this is used to reset the openIMU300ZI module
 *  @tparam drdy    Data ready pin name, this is used as indication of data readiness
 *  @tparam ss      Slave select pin name, this is used as slave select, pull low to initiate
 *                  communication process.
 *
 *  @todo   Attach to exti in the future, and make user able to choose
 *
 *  Example of using ImuDriver:
 *
 *  @code
 *  #include "imu_driver.hpp"
 *
 *  SPI     spi3(PB_5, PB_6, PB_3);     // declare SPI instance globally
 *  Serial  pc(USBTX, USBRX, 115200);   // print debug message
 *
 *  int main()
 *  {
 *      // SPI instance, reset, data ready, slave select
 *      ImuDriver<spi3, PA_10, PA_8, PA_9> imu(ImuExtiRcvNormalMsg);
 *
 *      while(true)
 *      {
 *          pc.printf("%.3f, %.3f, %.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]);
 *      }
 *  }
 *
 *  @endcode
 */
template <SPI& Spi, PinName rst, PinName drdy, PinName ss>
class ImuDriver
{
private:
    ImuExtiConfig m_extiConfig;

    SPI& m_spi;
    DigitalOut m_rst, m_ss;
    DigitalIn m_drdy;
    InterruptIn m_drdyExti;

    float m_gyroScaler[3];
    float m_accelScaler[3];
    float m_ahrsScaler[3];
    
    ImuDriver();    // cannot initialize via default constructor
    
    std::uint16_t m_imuSpiWrite(const std::uint16_t val) const
    {
        // RAII
        class SpiLock
        {
        private:
            SPI& m_spi;
        public:
            SpiLock(SPI& t_spi) : m_spi(t_spi)
            {
                m_spi.lock();
            }

            ~SpiLock()
            {
                m_spi.unlock();
            }
        };

        SpiLock lock(m_spi);
        return m_spi.write(val);
    }

    inline std::uint16_t m_spiGenerateReadCmd(const std::uint8_t t_val) const
    {
        return t_val << 8U;
    }

    inline bool m_spiIsDataReady()
    {
        return m_drdy.read() == 0;
    }

    inline void m_processImuRawData()
    {
        for (int i = 0; i < 3; ++i) {
            imuProcessedData.accel[i] = imuRawData.accel[i] / m_accelScaler[i];
            imuProcessedData.rate[i] = imuRawData.rate[i] / m_gyroScaler[i];
        }
    }

    inline void m_processAhrsRawData()
    {
        for (int i = 0; i < 3; ++i) {
            ahrsProcessedData.attitude[i] = ahrsRawData.attitude[i] / m_ahrsScaler[i];
        }
    }

    void m_dataReadyExtiCallback()
    {
        if (m_extiConfig == ImuExtiRcvBothMsg) {
            receiveBurstMsgImpl();
            wait_us(10);
            receiveAhrsMsgImpl();
        } else if (m_extiConfig == ImuExtiRcvNormalMsg) {
            receiveBurstMsgImpl();
        } else if (m_extiConfig == ImuExtiRcvAhrsMsg) {
            receiveAhrsMsgImpl();
        }
    }

    ImuDriverStatus receiveBurstMsgImpl(bool t_extended = false)
    {
        if (m_spiIsDataReady()) {
            std::uint16_t max_data = 0U;
            std::uint16_t burst_reg = 0U;
            std::uint8_t data_rcved = 0U;

            if (!t_extended) {
                max_data = 8U;
                burst_reg = m_spiGenerateReadCmd(ReadBurstDataRegister);
            } else {
                max_data = 11U;
                burst_reg = m_spiGenerateReadCmd(ReadExtBurstDataRegister);
            }

            m_ss.write(0);  // start transfer
            m_imuSpiWrite(burst_reg);
            
            while(data_rcved < max_data) {
                const std::uint16_t spi_data = m_imuSpiWrite(0x0000);
                switch(data_rcved) {
                    case 0:
                        imuRawData.status = spi_data;
                        break;
                    case 1:
                    case 2:
                    case 3:
                        imuRawData.rate[data_rcved - 1] = spi_data;
                        break;
                    case 4:
                    case 5:
                    case 6:
                        imuRawData.accel[data_rcved - 4] = spi_data;
                        break;
                    case 7:
                        imuRawData.temperature = spi_data;
                        break;
                    default:
                        break;
                }

                ++data_rcved;
            }

            m_ss.write(1);

            m_processImuRawData();
            return ImuDriverStatusOK;
        } else {
            return ImuDriverStatusDataNotReady;
        }
    }

    ImuDriverStatus receiveAhrsMsgImpl()
    {
        if (m_spiIsDataReady()) {
            std::uint8_t data_rcved = 0U;
            const std::uint8_t max_data = 5U;
            const std::uint16_t ahrs_reg = m_spiGenerateReadCmd(ReadAhrsBurstDataRegister);

            m_ss.write(0);
            m_imuSpiWrite(ahrs_reg);

            while(data_rcved < max_data) {
                const std::uint16_t spi_data = m_imuSpiWrite(0x0000);
                switch(data_rcved) {
                    case 0:
                        ahrsRawData.status = spi_data;
                        break;
                    case 1:
                    case 2:
                    case 3:
                        ahrsRawData.attitude[data_rcved - 1] = spi_data;
                        break;
                    case 4:
                        ahrsRawData.temperature = spi_data;
                        break;
                    default:
                        break;
                }

                ++data_rcved;

            }

            m_ss.write(1);

            m_processAhrsRawData();
            return ImuDriverStatusOK;
        } else {
            return ImuDriverStatusDataNotReady;
        }
    }
public:
    /**
     *  imuRawData contains raw data received from @ref receiveBurstMsg(bool t_extended)
     */
    ImuRawData imuRawData;

    /**
     *  imuProcessedData contains processed data, which is merely scaling operation
     */
    ImuProcessedData imuProcessedData;

    /**
     *  ahrsRawaData contains raw data received from @ref receiveAhrsMsg()
     */
    AhrsRawData ahrsRawData;

    /**
     *  ahrsProcessedData contains processed data, which is merely scaling operation
     */
    AhrsProcessedData ahrsProcessedData;

    static const int DEFAULT_IMU_ACCEL_SCALER = 4000;
    static const int DEFAULT_IMU_GYRO_SCALER = 100;
    static const int DEFAULT_AHRS_ATTITUDE_SCALER = 90;

    /**
     *
     */
    explicit ImuDriver(const ImuExtiConfig t_exti_config) : m_extiConfig(t_exti_config), m_spi(Spi), m_rst(rst), m_ss(ss), m_drdy(drdy), m_drdyExti(drdy)
    {
        for (int i = 0; i < 3; ++i) {
            m_gyroScaler[i] = static_cast<float>(DEFAULT_IMU_GYRO_SCALER);      // what an idiotic way of initialization LUL
            m_accelScaler[i] = static_cast<float>(DEFAULT_IMU_ACCEL_SCALER);    // Oh, I forgot that mbed os2 still stucks at c++98,
            m_ahrsScaler[i] = static_cast<float>(DEFAULT_AHRS_ATTITUDE_SCALER); // how unfortunate LUL
        }

        if (t_exti_config != ImuExtiRcvNotUsed) {
            m_drdyExti.fall(this, &ImuDriver<Spi, rst, drdy, ss>::m_dataReadyExtiCallback);
            m_drdyExti.enable_irq();
        }

        m_spi.format(16, 3);
        m_spi.frequency();
    }

    /**
     *  @brief  This function handles the receiving of burst message function, burst message
     *          contains accelerometer (g), gyroscope (deg/s), status and temperature data (Celcius)
     *
     *  @param  t_extended  bool to indicate the message type is extended or not, default false
     *
     *  @return ImuDriverStatus to indicate the result of the receive operation
     */
    ImuDriverStatus receiveBurstMsg(bool t_extended = false)
    {
        return (m_extiConfig & ImuExtiRcvNormalMsg ? ImuDriverStatusInvalidCall : receiveBurstMsgImpl(t_extended));
    }

    /**
     *  @brief  This function handles the receiving of AHRS burst message function, AHRS
     *          burst message contains the attitude in euler angle (deg).
     *
     *  @return ImuDriverStatus to indicate the result of the receive operation
     *
     *  @note   This is only suitable for AHRS app, and additional procedure needs to be done
     *          in order to make it work
     */
    ImuDriverStatus receiveAhrsMsg()
    {
        return (m_extiConfig & ImuExtiRcvAhrsMsg ? ImuDriverStatusInvalidCall : receiveAhrsMsgImpl());
    }

    /**
     *  @brief  This function handles the external interrupt option, whether receive AHRS 
     *          burst message or normal imu message.
     *
     *  @return ImuDriverStatus to indicate the result of the receive operation
     *
     *  @note   Never try receiving two messages at the same time
     */
    ImuDriverStatus configExti(ImuExtiConfig const& t_exti_config)
    {
        m_extiConfig = t_exti_config;

        if (m_extoConfig == ImuExtiRcvNotUsed) {
            m_drdyExti.disable_irq();
        } else {
            m_drdyExti.enable_irq();
        }

        return ImuDriverStatusOK;
    }
};

#endif  // IMU_DRIVER_HPP_