Jacky Tseng / Mbed 2 deprecated imu_driver

handle master side communication of openIMU300ZI module

Dependencies:   mbed

Dependents:   VDU_2021

imu_driver.hpp@5:e71931fcae33, 2019-11-17 (annotated)

Committer:
Arithemetica
Date:
Sun Nov 17 07:06:47 2019 +0000
Revision:
5:e71931fcae33
Parent:
4:a2becd590d99
Child:
7:224ffdacb240
Update documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arithemetica 1:8e31413068af 1 #ifndef IMU_DRIVER_HPP_
Arithemetica 1:8e31413068af 2 #define IMU_DRIVER_HPP_
Arithemetica 1:8e31413068af 3
Arithemetica 0:8c01a98a2812 4 #include <mbed.h>
Arithemetica 0:8c01a98a2812 5 #include <cstdint>
Arithemetica 0:8c01a98a2812 6
Arithemetica 0:8c01a98a2812 7 typedef enum ImuDriverStatus {
Arithemetica 0:8c01a98a2812 8 ImuDriverStatusOK,
Arithemetica 0:8c01a98a2812 9 ImuDriverStatusDataNotReady
Arithemetica 0:8c01a98a2812 10 } ImuDriverStatus;
Arithemetica 0:8c01a98a2812 11
Arithemetica 0:8c01a98a2812 12 typedef enum ImuDriverRegister {
Arithemetica 0:8c01a98a2812 13 ReadAhrsBurstDataRegister = 0x3D,
Arithemetica 0:8c01a98a2812 14 ReadBurstDataRegister = 0x3E,
Arithemetica 0:8c01a98a2812 15 ReadExtBurstDataRegister = 0x3F
Arithemetica 0:8c01a98a2812 16 } ImuDriverRegister;
Arithemetica 0:8c01a98a2812 17
Arithemetica 0:8c01a98a2812 18 typedef struct AhrsRawData {
Arithemetica 0:8c01a98a2812 19 std::uint16_t status;
Arithemetica 0:8c01a98a2812 20 std::int16_t attitude[3];
Arithemetica 0:8c01a98a2812 21 std::int16_t temperature;
Arithemetica 0:8c01a98a2812 22 } AhrsRawData;
Arithemetica 0:8c01a98a2812 23
Arithemetica 0:8c01a98a2812 24 typedef struct AhrsProcessedData {
Arithemetica 0:8c01a98a2812 25 float attitude[3];
Arithemetica 0:8c01a98a2812 26 } AhrsProcessedData;
Arithemetica 0:8c01a98a2812 27
Arithemetica 0:8c01a98a2812 28 typedef struct ImuRawData {
Arithemetica 0:8c01a98a2812 29 std::uint16_t status;
Arithemetica 0:8c01a98a2812 30 std::int16_t rate[3];
Arithemetica 0:8c01a98a2812 31 std::int16_t accel[3];
Arithemetica 0:8c01a98a2812 32 std::int16_t temperature;
Arithemetica 0:8c01a98a2812 33 } ImuRawData;
Arithemetica 0:8c01a98a2812 34
Arithemetica 0:8c01a98a2812 35 typedef struct ImuProcessedData {
Arithemetica 0:8c01a98a2812 36 std::uint16_t status;
Arithemetica 0:8c01a98a2812 37
Arithemetica 0:8c01a98a2812 38 float rate[3];
Arithemetica 0:8c01a98a2812 39 float accel[3];
Arithemetica 0:8c01a98a2812 40
Arithemetica 0:8c01a98a2812 41 std::int16_t temperature;
Arithemetica 0:8c01a98a2812 42 } ImuProcessedData;
Arithemetica 0:8c01a98a2812 43
Arithemetica 0:8c01a98a2812 44 /**
Arithemetica 2:f3a7def7a7e1 45 * @brief ImuDriver is used to handle the SPI master side of openIMU300ZI module, the
Arithemetica 2:f3a7def7a7e1 46 * default setups are:
Arithemetica 0:8c01a98a2812 47 *
Arithemetica 2:f3a7def7a7e1 48 * - Data frame: 16 bits
Arithemetica 2:f3a7def7a7e1 49 * - CPOL: High (1)
Arithemetica 2:f3a7def7a7e1 50 * - CPHA: 2 Edge (1)
Arithemetica 2:f3a7def7a7e1 51 * - Frequency: 1.40625 MHz (Baudrate prescaler 32 for NUCLEO-F446RE under 180MHz)
Arithemetica 2:f3a7def7a7e1 52 * - Endian: MSB first (Mbed only support MSB AFAIK)
Arithemetica 5:e71931fcae33 53 *
Arithemetica 3:8552e26cd162 54 * According to [openIMU300ZI SPI framework](https://openimu.readthedocs.io/en/latest/software/SPImessaging.html):
Arithemetica 5:e71931fcae33 55 *
Arithemetica 3:8552e26cd162 56 * - Data transferred in 16-bit word-length and MSB-first
Arithemetica 5:e71931fcae33 57 * - fCLK ≤ 2.0 MHz
Arithemetica 3:8552e26cd162 58 * - CPOL = 1 (clock polarity) and CPHA = 1 (clock phase)
Arithemetica 0:8c01a98a2812 59 *
Arithemetica 2:f3a7def7a7e1 60 * @tparam spi SPI comm instance, ImuDriver currently support only SPI framework
Arithemetica 2:f3a7def7a7e1 61 * @tparam rst Reset pin name, this is used to reset the openIMU300ZI module
Arithemetica 2:f3a7def7a7e1 62 * @tparam drdy Data ready pin name, this is used as indication of data readiness
Arithemetica 2:f3a7def7a7e1 63 * (@todo Attach to interrupt in the future, and make user able to choose)
Arithemetica 2:f3a7def7a7e1 64 * @tparam ss Slave select pin name, this is used as slave select, pull low to initiate
Arithemetica 2:f3a7def7a7e1 65 * communication process.
Arithemetica 5:e71931fcae33 66 *
Arithemetica 0:8c01a98a2812 67 * Example of using ImuDriver:
Arithemetica 0:8c01a98a2812 68 *
Arithemetica 0:8c01a98a2812 69 * @code
Arithemetica 0:8c01a98a2812 70 * #include "imu_driver.hpp"
Arithemetica 0:8c01a98a2812 71 *
Arithemetica 0:8c01a98a2812 72 * SPI spi3(PB_5, PB_6, PB_3); // declare SPI instance globally
Arithemetica 0:8c01a98a2812 73 * Serial pc(USBTX, USBRX, 115200); // print debug message
Arithemetica 0:8c01a98a2812 74 *
Arithemetica 0:8c01a98a2812 75 * int main()
Arithemetica 0:8c01a98a2812 76 * {
Arithemetica 5:e71931fcae33 77 * // SPI instance, reset, data ready, slave select
Arithemetica 0:8c01a98a2812 78 * ImuDriver<spi3, PA_10, PA_8, PA_9> imu;
Arithemetica 0:8c01a98a2812 79 *
Arithemetica 0:8c01a98a2812 80 * while(true)
Arithemetica 0:8c01a98a2812 81 * {
Arithemetica 0:8c01a98a2812 82 * if (imu.receiveBurstMsg() == ImuDriverStatusOK)
Arithemetica 0:8c01a98a2812 83 * {
Arithemetica 0:8c01a98a2812 84 * pc.printf("%.3f, %.3f, %.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]);
Arithemetica 0:8c01a98a2812 85 * }
Arithemetica 0:8c01a98a2812 86 * }
Arithemetica 0:8c01a98a2812 87 * }
Arithemetica 5:e71931fcae33 88 *
Arithemetica 0:8c01a98a2812 89 * @endcode
Arithemetica 0:8c01a98a2812 90 */
Arithemetica 2:f3a7def7a7e1 91 template <SPI& Spi, PinName rst, PinName drdy, PinName ss>
Arithemetica 0:8c01a98a2812 92 class ImuDriver
Arithemetica 0:8c01a98a2812 93 {
Arithemetica 0:8c01a98a2812 94 private:
Arithemetica 2:f3a7def7a7e1 95 typedef ImuDriver<Spi, rst, drdy, ss> ImuDriverType;
Arithemetica 2:f3a7def7a7e1 96 typedef ImuDriverStatus (ImuDriver<Spi, rst, drdy, ss>::*DrdyCallback)();
Arithemetica 2:f3a7def7a7e1 97
Arithemetica 2:f3a7def7a7e1 98
Arithemetica 0:8c01a98a2812 99 SPI& m_spi;
Arithemetica 0:8c01a98a2812 100 DigitalOut m_rst, m_ss;
Arithemetica 0:8c01a98a2812 101 DigitalIn m_drdy;
Arithemetica 2:f3a7def7a7e1 102 InterruptIn m_drdyExti;
Arithemetica 5:e71931fcae33 103
Arithemetica 0:8c01a98a2812 104 float m_gyroScaler[3];
Arithemetica 0:8c01a98a2812 105 float m_accelScaler[3];
Arithemetica 0:8c01a98a2812 106 float m_ahrsScaler[3];
Arithemetica 0:8c01a98a2812 107 private:
Arithemetica 0:8c01a98a2812 108 std::uint16_t m_imuSpiWrite(const std::uint16_t val) const
Arithemetica 0:8c01a98a2812 109 {
Arithemetica 0:8c01a98a2812 110 // RAII
Arithemetica 0:8c01a98a2812 111 class SpiLock
Arithemetica 0:8c01a98a2812 112 {
Arithemetica 0:8c01a98a2812 113 private:
Arithemetica 0:8c01a98a2812 114 SPI& m_spi;
Arithemetica 0:8c01a98a2812 115 public:
Arithemetica 0:8c01a98a2812 116 SpiLock(SPI& t_spi) : m_spi(t_spi)
Arithemetica 0:8c01a98a2812 117 {
Arithemetica 0:8c01a98a2812 118 m_spi.lock();
Arithemetica 0:8c01a98a2812 119 }
Arithemetica 0:8c01a98a2812 120
Arithemetica 0:8c01a98a2812 121 ~SpiLock()
Arithemetica 0:8c01a98a2812 122 {
Arithemetica 0:8c01a98a2812 123 m_spi.unlock();
Arithemetica 0:8c01a98a2812 124 }
Arithemetica 0:8c01a98a2812 125 };
Arithemetica 0:8c01a98a2812 126
Arithemetica 0:8c01a98a2812 127 SpiLock lock(m_spi);
Arithemetica 0:8c01a98a2812 128 return m_spi.write(val);
Arithemetica 0:8c01a98a2812 129 }
Arithemetica 0:8c01a98a2812 130
Arithemetica 0:8c01a98a2812 131 inline std::uint16_t m_spiGenerateReadCmd(const std::uint8_t t_val) const
Arithemetica 0:8c01a98a2812 132 {
Arithemetica 0:8c01a98a2812 133 return t_val << 8U;
Arithemetica 0:8c01a98a2812 134 }
Arithemetica 0:8c01a98a2812 135
Arithemetica 0:8c01a98a2812 136 inline bool m_spiIsDataReady()
Arithemetica 0:8c01a98a2812 137 {
Arithemetica 0:8c01a98a2812 138 return m_drdy.read() == 0;
Arithemetica 0:8c01a98a2812 139 }
Arithemetica 0:8c01a98a2812 140
Arithemetica 0:8c01a98a2812 141 inline void m_processImuRawData()
Arithemetica 0:8c01a98a2812 142 {
Arithemetica 0:8c01a98a2812 143 for (int i = 0; i < 3; ++i) {
Arithemetica 0:8c01a98a2812 144 imuProcessedData.accel[i] = imuRawData.accel[i] / m_accelScaler[i];
Arithemetica 0:8c01a98a2812 145 imuProcessedData.rate[i] = imuRawData.rate[i] / m_gyroScaler[i];
Arithemetica 0:8c01a98a2812 146 }
Arithemetica 0:8c01a98a2812 147 }
Arithemetica 0:8c01a98a2812 148
Arithemetica 0:8c01a98a2812 149 inline void m_processAhrsRawData()
Arithemetica 0:8c01a98a2812 150 {
Arithemetica 0:8c01a98a2812 151 for (int i = 0; i < 3; ++i) {
Arithemetica 0:8c01a98a2812 152 ahrsProcessedData.attitude[i] = ahrsRawData.attitude[i] / m_ahrsScaler[i];
Arithemetica 0:8c01a98a2812 153 }
Arithemetica 0:8c01a98a2812 154 }
Arithemetica 5:e71931fcae33 155
Arithemetica 2:f3a7def7a7e1 156 void m_receiveAhrsCallback()
Arithemetica 2:f3a7def7a7e1 157 {
Arithemetica 2:f3a7def7a7e1 158 receiveAhrsMsg();
Arithemetica 2:f3a7def7a7e1 159 }
Arithemetica 0:8c01a98a2812 160 public:
Arithemetica 0:8c01a98a2812 161 ImuRawData imuRawData;
Arithemetica 0:8c01a98a2812 162 ImuProcessedData imuProcessedData;
Arithemetica 0:8c01a98a2812 163
Arithemetica 0:8c01a98a2812 164 AhrsRawData ahrsRawData;
Arithemetica 0:8c01a98a2812 165 AhrsProcessedData ahrsProcessedData;
Arithemetica 0:8c01a98a2812 166
Arithemetica 0:8c01a98a2812 167 static const int DEFAULT_IMU_ACCEL_SCALER = 4000;
Arithemetica 0:8c01a98a2812 168 static const int DEFAULT_IMU_GYRO_SCALER = 200;
Arithemetica 0:8c01a98a2812 169 static const int DEFAULT_AHRS_ATTITUDE_SCALER = 90;
Arithemetica 0:8c01a98a2812 170 public:
Arithemetica 2:f3a7def7a7e1 171 ImuDriver() : m_spi(Spi), m_rst(rst), m_ss(ss), m_drdy(drdy), m_drdyExti(drdy)
Arithemetica 0:8c01a98a2812 172 {
Arithemetica 0:8c01a98a2812 173 for (int i = 0; i < 3; ++i) {
Arithemetica 0:8c01a98a2812 174 m_gyroScaler[i] = static_cast<float>(DEFAULT_IMU_GYRO_SCALER); // what an idiotic way of initialization LUL
Arithemetica 2:f3a7def7a7e1 175 m_accelScaler[i] = static_cast<float>(DEFAULT_IMU_ACCEL_SCALER); // Oh, I forgot that mbed os2 still stucks at c++98,
Arithemetica 0:8c01a98a2812 176 m_ahrsScaler[i] = static_cast<float>(DEFAULT_AHRS_ATTITUDE_SCALER); // how unfortunate LUL
Arithemetica 0:8c01a98a2812 177 }
Arithemetica 0:8c01a98a2812 178
Arithemetica 2:f3a7def7a7e1 179 // m_drdyExti.rise(this, &ImuDriver<Spi, rst, drdy, ss>::m_receiveAhrsCallback);
Arithemetica 0:8c01a98a2812 180 m_spi.format(16, 3);
Arithemetica 0:8c01a98a2812 181 m_spi.frequency(1406250);
Arithemetica 0:8c01a98a2812 182 }
Arithemetica 0:8c01a98a2812 183
Arithemetica 0:8c01a98a2812 184 /**
Arithemetica 5:e71931fcae33 185 * @brief This function handles the receiving of burst message function, burst message
Arithemetica 5:e71931fcae33 186 * contains accelerometer, gyroscope, status and temperature data
Arithemetica 0:8c01a98a2812 187 *
Arithemetica 0:8c01a98a2812 188 * @param t_extended bool to indicate the message type is extended or not, default false
Arithemetica 0:8c01a98a2812 189 *
Arithemetica 0:8c01a98a2812 190 * @return ImuDriverStatus to indicate the result of the receive operation
Arithemetica 0:8c01a98a2812 191 */
Arithemetica 0:8c01a98a2812 192 ImuDriverStatus receiveBurstMsg(bool t_extended = false)
Arithemetica 0:8c01a98a2812 193 {
Arithemetica 0:8c01a98a2812 194 if (m_spiIsDataReady()) {
Arithemetica 0:8c01a98a2812 195 std::uint16_t max_data = 0U;
Arithemetica 0:8c01a98a2812 196 std::uint16_t burst_reg = 0U;
Arithemetica 0:8c01a98a2812 197
Arithemetica 0:8c01a98a2812 198 if (!t_extended) {
Arithemetica 0:8c01a98a2812 199 max_data = 8U;
Arithemetica 0:8c01a98a2812 200 burst_reg = m_spiGenerateReadCmd(ReadBurstDataRegister);
Arithemetica 0:8c01a98a2812 201 } else {
Arithemetica 0:8c01a98a2812 202 max_data = 11U;
Arithemetica 0:8c01a98a2812 203 burst_reg = m_spiGenerateReadCmd(ReadExtBurstDataRegister);
Arithemetica 0:8c01a98a2812 204 }
Arithemetica 0:8c01a98a2812 205
Arithemetica 0:8c01a98a2812 206 m_ss.write(0); // start transfer
Arithemetica 0:8c01a98a2812 207 m_imuSpiWrite(burst_reg);
Arithemetica 0:8c01a98a2812 208
Arithemetica 0:8c01a98a2812 209 static std::uint8_t data_rcved = 0U;
Arithemetica 0:8c01a98a2812 210
Arithemetica 0:8c01a98a2812 211 while(data_rcved < max_data) {
Arithemetica 0:8c01a98a2812 212 const std::uint16_t spi_data = m_imuSpiWrite(0x0000);
Arithemetica 0:8c01a98a2812 213 switch(data_rcved) {
Arithemetica 0:8c01a98a2812 214 case 0:
Arithemetica 0:8c01a98a2812 215 imuRawData.status = spi_data;
Arithemetica 0:8c01a98a2812 216 break;
Arithemetica 0:8c01a98a2812 217 case 1:
Arithemetica 0:8c01a98a2812 218 case 2:
Arithemetica 0:8c01a98a2812 219 case 3:
Arithemetica 0:8c01a98a2812 220 imuRawData.rate[data_rcved - 1] = spi_data;
Arithemetica 0:8c01a98a2812 221 break;
Arithemetica 0:8c01a98a2812 222 case 4:
Arithemetica 0:8c01a98a2812 223 case 5:
Arithemetica 0:8c01a98a2812 224 case 6:
Arithemetica 0:8c01a98a2812 225 imuRawData.accel[data_rcved - 4] = spi_data;
Arithemetica 0:8c01a98a2812 226 break;
Arithemetica 0:8c01a98a2812 227 case 7:
Arithemetica 0:8c01a98a2812 228 imuRawData.temperature = spi_data;
Arithemetica 0:8c01a98a2812 229 break;
Arithemetica 0:8c01a98a2812 230 default:
Arithemetica 0:8c01a98a2812 231 break;
Arithemetica 0:8c01a98a2812 232 }
Arithemetica 0:8c01a98a2812 233
Arithemetica 0:8c01a98a2812 234 ++data_rcved;
Arithemetica 0:8c01a98a2812 235 }
Arithemetica 0:8c01a98a2812 236
Arithemetica 0:8c01a98a2812 237 data_rcved = 0U;
Arithemetica 0:8c01a98a2812 238 m_ss.write(1);
Arithemetica 0:8c01a98a2812 239
Arithemetica 0:8c01a98a2812 240 m_processImuRawData();
Arithemetica 0:8c01a98a2812 241 return ImuDriverStatusOK;
Arithemetica 0:8c01a98a2812 242 } else {
Arithemetica 0:8c01a98a2812 243 return ImuDriverStatusDataNotReady;
Arithemetica 0:8c01a98a2812 244 }
Arithemetica 0:8c01a98a2812 245 }
Arithemetica 0:8c01a98a2812 246
Arithemetica 0:8c01a98a2812 247 /**
Arithemetica 5:e71931fcae33 248 * @brief This function handles the receiving of AHRS burst message function, AHRS
Arithemetica 5:e71931fcae33 249 * burst message contains the attitude in euler angle.
Arithemetica 0:8c01a98a2812 250 *
Arithemetica 0:8c01a98a2812 251 * @return ImuDriverStatus to indicate the result of the receive operation
Arithemetica 0:8c01a98a2812 252 *
Arithemetica 0:8c01a98a2812 253 * @note This is only suitable for AHRS app, and additional procedure needs to be done
Arithemetica 0:8c01a98a2812 254 * in order to make it work
Arithemetica 0:8c01a98a2812 255 */
Arithemetica 0:8c01a98a2812 256 ImuDriverStatus receiveAhrsMsg()
Arithemetica 0:8c01a98a2812 257 {
Arithemetica 0:8c01a98a2812 258 if (m_spiIsDataReady()) {
Arithemetica 0:8c01a98a2812 259 static std::uint8_t data_rcved = 0U;
Arithemetica 0:8c01a98a2812 260 const std::uint8_t max_data = 5U;
Arithemetica 0:8c01a98a2812 261 const std::uint16_t ahrs_reg = m_spiGenerateReadCmd(ReadAhrsBurstDataRegister);
Arithemetica 0:8c01a98a2812 262
Arithemetica 0:8c01a98a2812 263 m_ss.write(0);
Arithemetica 0:8c01a98a2812 264 m_imuSpiWrite(ahrs_reg);
Arithemetica 0:8c01a98a2812 265
Arithemetica 0:8c01a98a2812 266 while(data_rcved < max_data) {
Arithemetica 0:8c01a98a2812 267 const std::uint16_t spi_data = m_imuSpiWrite(0x0000);
Arithemetica 0:8c01a98a2812 268 switch(data_rcved) {
Arithemetica 0:8c01a98a2812 269 case 0:
Arithemetica 0:8c01a98a2812 270 ahrsRawData.status = spi_data;
Arithemetica 0:8c01a98a2812 271 break;
Arithemetica 0:8c01a98a2812 272 case 1:
Arithemetica 0:8c01a98a2812 273 case 2:
Arithemetica 0:8c01a98a2812 274 case 3:
Arithemetica 0:8c01a98a2812 275 ahrsRawData.attitude[data_rcved - 1] = spi_data;
Arithemetica 0:8c01a98a2812 276 break;
Arithemetica 0:8c01a98a2812 277 case 4:
Arithemetica 0:8c01a98a2812 278 ahrsRawData.temperature = spi_data;
Arithemetica 0:8c01a98a2812 279 break;
Arithemetica 0:8c01a98a2812 280 default:
Arithemetica 0:8c01a98a2812 281 break;
Arithemetica 0:8c01a98a2812 282 }
Arithemetica 0:8c01a98a2812 283
Arithemetica 0:8c01a98a2812 284 ++data_rcved;
Arithemetica 0:8c01a98a2812 285 }
Arithemetica 0:8c01a98a2812 286
Arithemetica 0:8c01a98a2812 287 data_rcved = 0U;
Arithemetica 0:8c01a98a2812 288 m_ss.write(1);
Arithemetica 0:8c01a98a2812 289
Arithemetica 0:8c01a98a2812 290 m_processAhrsRawData();
Arithemetica 0:8c01a98a2812 291 return ImuDriverStatusOK;
Arithemetica 0:8c01a98a2812 292 } else {
Arithemetica 0:8c01a98a2812 293 return ImuDriverStatusDataNotReady;
Arithemetica 0:8c01a98a2812 294 }
Arithemetica 0:8c01a98a2812 295 }
Arithemetica 1:8e31413068af 296 };
Arithemetica 1:8e31413068af 297
Arithemetica 1:8e31413068af 298 #endif // IMU_DRIVER_HPP_