File content as of revision 2:f3a7def7a7e1:

#ifndef IMU_DRIVER_HPP_
#define IMU_DRIVER_HPP_

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

typedef enum ImuDriverStatus {
    ImuDriverStatusOK,
    ImuDriverStatusDataNotReady
} 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;

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;

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 SPI master side of openIMU300ZI module, the
 *         default setups are:
 *
 *           - Data frame: 16 bits
 *           - CPOL: High (1)
 *           - CPHA: 2 Edge (1)
 *           - Frequency: 1.40625 MHz (Baudrate prescaler 32 for NUCLEO-F446RE under 180MHz)
 *           - Endian: MSB first (Mbed only support MSB AFAIK)
 *
 *  @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
 *                  (@todo Attach to interrupt in the future, and make user able to choose)
 *  @tparam ss      Slave select pin name, this is used as slave select, pull low to initiate
 *                  communication process.
 *  
 *  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;
 *
 *      while(true)
 *      {
 *          if (imu.receiveBurstMsg() == ImuDriverStatusOK)
 *          {
 *              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:
    typedef ImuDriver<Spi, rst, drdy, ss> ImuDriverType;
    typedef ImuDriverStatus (ImuDriver<Spi, rst, drdy, ss>::*DrdyCallback)();


    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];
private:
    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_receiveAhrsCallback()
    {
        receiveAhrsMsg();
    }
public:
    ImuRawData imuRawData;
    ImuProcessedData imuProcessedData;

    AhrsRawData ahrsRawData;
    AhrsProcessedData ahrsProcessedData;

    static const int DEFAULT_IMU_ACCEL_SCALER = 4000;
    static const int DEFAULT_IMU_GYRO_SCALER = 200;
    static const int DEFAULT_AHRS_ATTITUDE_SCALER = 90;
public:
    ImuDriver() : 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
        }

//        m_drdyExti.rise(this, &ImuDriver<Spi, rst, drdy, ss>::m_receiveAhrsCallback);
        m_spi.format(16, 3);
        m_spi.frequency(1406250);
    }

    /**
     *  @brief  This function handles the receiving of burst message function
     *
     *  @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)
    {
        if (m_spiIsDataReady()) {
            std::uint16_t max_data = 0U;
            std::uint16_t burst_reg = 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);

            static std::uint8_t data_rcved = 0U;

            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;
            }

            data_rcved = 0U;
            m_ss.write(1);

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

    /**
     *  @brief  This function handles the receiving of AHRS burst message function
     *
     *  @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()
    {
        if (m_spiIsDataReady()) {
            static 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;
            }

            data_rcved = 0U;
            m_ss.write(1);

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

#endif  // IMU_DRIVER_HPP_