Jacky Tseng / Mbed 2 deprecated imu_driver

handle master side communication of openIMU300ZI module

Dependencies:   mbed

Dependents:   VDU_2021

imu_driver.hpp@0:8c01a98a2812, 2019-11-16 (annotated)

Committer:
Arithemetica
Date:
Sat Nov 16 15:44:38 2019 +0000
Revision:
0:8c01a98a2812
Child:
1:8e31413068af
Initial commit

Who changed what in which revision?

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