Jacky Tseng
/
imu_driver
handle master side communication of openIMU300ZI module
imu_driver.hpp@20:19f5c94f8660, 2019-12-18 (annotated)
- Committer:
- Arithemetica
- Date:
- Wed Dec 18 06:55:18 2019 +0000
- Revision:
- 20:19f5c94f8660
- Parent:
- 19:3ee54f9a56f3
- Child:
- 21:9430046ebd9d
remove main.cpp
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Arithemetica | 7:224ffdacb240 | 1 | /** |
Arithemetica | 7:224ffdacb240 | 2 | * @file imu_driver.hpp |
Arithemetica | 10:d8223969c541 | 3 | * @brief ImuDriver class and other data storage struct definition. The C language version |
Arithemetica | 10:d8223969c541 | 4 | * can be found at <A HREF="https://github.com/osjacky430/imu_driver">this github website</A> |
Arithemetica | 7:224ffdacb240 | 5 | */ |
Arithemetica | 7:224ffdacb240 | 6 | |
Arithemetica | 1:8e31413068af | 7 | #ifndef IMU_DRIVER_HPP_ |
Arithemetica | 1:8e31413068af | 8 | #define IMU_DRIVER_HPP_ |
Arithemetica | 1:8e31413068af | 9 | |
Arithemetica | 0:8c01a98a2812 | 10 | #include <mbed.h> |
Arithemetica | 0:8c01a98a2812 | 11 | #include <cstdint> |
Arithemetica | 0:8c01a98a2812 | 12 | |
Arithemetica | 14:6c31a7ab45f0 | 13 | typedef enum ImuExtiConfig { |
Arithemetica | 14:6c31a7ab45f0 | 14 | ImuExtiRcvNotUsed = 0b00, |
Arithemetica | 14:6c31a7ab45f0 | 15 | ImuExtiRcvNormalMsg = 0b01, |
Arithemetica | 14:6c31a7ab45f0 | 16 | ImuExtiRcvAhrsMsg = 0b10 |
Arithemetica | 14:6c31a7ab45f0 | 17 | } ImuExtiConfig; |
Arithemetica | 14:6c31a7ab45f0 | 18 | |
Arithemetica | 20:19f5c94f8660 | 19 | /* this needs to be extended (IMO) */ |
Arithemetica | 19:3ee54f9a56f3 | 20 | inline ImuExtiConfig operator |(ImuExtiConfig const& left, ImuExtiConfig const& right) |
Arithemetica | 19:3ee54f9a56f3 | 21 | { |
Arithemetica | 19:3ee54f9a56f3 | 22 | return ImuExtiConfig(left | right); |
Arithemetica | 19:3ee54f9a56f3 | 23 | } |
Arithemetica | 19:3ee54f9a56f3 | 24 | |
Arithemetica | 17:629b2f317d0a | 25 | /** |
Arithemetica | 17:629b2f317d0a | 26 | * @brief ImuDriverStatus |
Arithemetica | 17:629b2f317d0a | 27 | */ |
Arithemetica | 12:bb490978e153 | 28 | typedef enum ImuDriverStatus { |
Arithemetica | 14:6c31a7ab45f0 | 29 | ImuDriverStatusOK, |
Arithemetica | 14:6c31a7ab45f0 | 30 | ImuDriverStatusDataNotReady, |
Arithemetica | 14:6c31a7ab45f0 | 31 | ImuDriverStatusInvalidCall |
Arithemetica | 14:6c31a7ab45f0 | 32 | } ImuDriverStatus; |
Arithemetica | 12:bb490978e153 | 33 | |
Arithemetica | 0:8c01a98a2812 | 34 | typedef enum ImuDriverRegister { |
Arithemetica | 0:8c01a98a2812 | 35 | ReadAhrsBurstDataRegister = 0x3D, |
Arithemetica | 0:8c01a98a2812 | 36 | ReadBurstDataRegister = 0x3E, |
Arithemetica | 0:8c01a98a2812 | 37 | ReadExtBurstDataRegister = 0x3F |
Arithemetica | 0:8c01a98a2812 | 38 | } ImuDriverRegister; |
Arithemetica | 0:8c01a98a2812 | 39 | |
Arithemetica | 0:8c01a98a2812 | 40 | typedef struct AhrsRawData { |
Arithemetica | 0:8c01a98a2812 | 41 | std::uint16_t status; |
Arithemetica | 0:8c01a98a2812 | 42 | std::int16_t attitude[3]; |
Arithemetica | 0:8c01a98a2812 | 43 | std::int16_t temperature; |
Arithemetica | 0:8c01a98a2812 | 44 | } AhrsRawData; |
Arithemetica | 0:8c01a98a2812 | 45 | |
Arithemetica | 10:d8223969c541 | 46 | /** |
Arithemetica | 12:bb490978e153 | 47 | * @brief AhrsProcessedData is data storage class fpr processed AHRS data |
Arithemetica | 14:6c31a7ab45f0 | 48 | * |
Arithemetica | 11:866dd73e4ab3 | 49 | * The data storage struct consists of: |
Arithemetica | 11:866dd73e4ab3 | 50 | * |
Arithemetica | 10:d8223969c541 | 51 | * - attitude: pitch, roll, yaw in deg, range from -180 ~ +180 deg |
Arithemetica | 10:d8223969c541 | 52 | */ |
Arithemetica | 0:8c01a98a2812 | 53 | typedef struct AhrsProcessedData { |
Arithemetica | 0:8c01a98a2812 | 54 | float attitude[3]; |
Arithemetica | 0:8c01a98a2812 | 55 | } AhrsProcessedData; |
Arithemetica | 0:8c01a98a2812 | 56 | |
Arithemetica | 0:8c01a98a2812 | 57 | typedef struct ImuRawData { |
Arithemetica | 0:8c01a98a2812 | 58 | std::uint16_t status; |
Arithemetica | 0:8c01a98a2812 | 59 | std::int16_t rate[3]; |
Arithemetica | 0:8c01a98a2812 | 60 | std::int16_t accel[3]; |
Arithemetica | 0:8c01a98a2812 | 61 | std::int16_t temperature; |
Arithemetica | 0:8c01a98a2812 | 62 | } ImuRawData; |
Arithemetica | 0:8c01a98a2812 | 63 | |
Arithemetica | 10:d8223969c541 | 64 | /** |
Arithemetica | 11:866dd73e4ab3 | 65 | * @brief ImuProcessedData is data storage class for processed imu data |
Arithemetica | 10:d8223969c541 | 66 | * |
Arithemetica | 11:866dd73e4ab3 | 67 | * This data storage class consists of: |
Arithemetica | 14:6c31a7ab45f0 | 68 | * |
Arithemetica | 10:d8223969c541 | 69 | * - status: if status = 0x0010 (over range error), status = 0 (no error) |
Arithemetica | 10:d8223969c541 | 70 | * - gyroscope: deg/s, range from -300 deg/s ~ 300 deg/s |
Arithemetica | 10:d8223969c541 | 71 | * - accelerometer: g, range from -4.5g ~ 4.5g |
Arithemetica | 10:d8223969c541 | 72 | * - temperature: Celcius |
Arithemetica | 10:d8223969c541 | 73 | */ |
Arithemetica | 0:8c01a98a2812 | 74 | typedef struct ImuProcessedData { |
Arithemetica | 0:8c01a98a2812 | 75 | std::uint16_t status; |
Arithemetica | 0:8c01a98a2812 | 76 | |
Arithemetica | 0:8c01a98a2812 | 77 | float rate[3]; |
Arithemetica | 0:8c01a98a2812 | 78 | float accel[3]; |
Arithemetica | 0:8c01a98a2812 | 79 | |
Arithemetica | 0:8c01a98a2812 | 80 | std::int16_t temperature; |
Arithemetica | 0:8c01a98a2812 | 81 | } ImuProcessedData; |
Arithemetica | 0:8c01a98a2812 | 82 | |
Arithemetica | 0:8c01a98a2812 | 83 | /** |
Arithemetica | 12:bb490978e153 | 84 | * @brief ImuDriver is used to handle the master side of SPI communication of openIMU300ZI module, the |
Arithemetica | 2:f3a7def7a7e1 | 85 | * default setups are: |
Arithemetica | 0:8c01a98a2812 | 86 | * |
Arithemetica | 2:f3a7def7a7e1 | 87 | * - Data frame: 16 bits |
Arithemetica | 2:f3a7def7a7e1 | 88 | * - CPOL: High (1) |
Arithemetica | 2:f3a7def7a7e1 | 89 | * - CPHA: 2 Edge (1) |
Arithemetica | 17:629b2f317d0a | 90 | * - Frequency: 1 MHz (Default frequency of spi) |
Arithemetica | 2:f3a7def7a7e1 | 91 | * - Endian: MSB first (Mbed only support MSB AFAIK) |
Arithemetica | 5:e71931fcae33 | 92 | * |
Arithemetica | 10:d8223969c541 | 93 | * According to <A HREF="https://openimu.readthedocs.io/en/latest/software/SPImessaging.html">openIMU300ZI SPI framework</A>: |
Arithemetica | 5:e71931fcae33 | 94 | * |
Arithemetica | 3:8552e26cd162 | 95 | * - Data transferred in 16-bit word-length and MSB-first |
Arithemetica | 5:e71931fcae33 | 96 | * - fCLK ≤ 2.0 MHz |
Arithemetica | 3:8552e26cd162 | 97 | * - CPOL = 1 (clock polarity) and CPHA = 1 (clock phase) |
Arithemetica | 0:8c01a98a2812 | 98 | * |
Arithemetica | 2:f3a7def7a7e1 | 99 | * @tparam spi SPI comm instance, ImuDriver currently support only SPI framework |
Arithemetica | 2:f3a7def7a7e1 | 100 | * @tparam rst Reset pin name, this is used to reset the openIMU300ZI module |
Arithemetica | 2:f3a7def7a7e1 | 101 | * @tparam drdy Data ready pin name, this is used as indication of data readiness |
Arithemetica | 2:f3a7def7a7e1 | 102 | * @tparam ss Slave select pin name, this is used as slave select, pull low to initiate |
Arithemetica | 2:f3a7def7a7e1 | 103 | * communication process. |
Arithemetica | 5:e71931fcae33 | 104 | * |
Arithemetica | 7:224ffdacb240 | 105 | * @todo Attach to exti in the future, and make user able to choose |
Arithemetica | 7:224ffdacb240 | 106 | * |
Arithemetica | 0:8c01a98a2812 | 107 | * Example of using ImuDriver: |
Arithemetica | 0:8c01a98a2812 | 108 | * |
Arithemetica | 0:8c01a98a2812 | 109 | * @code |
Arithemetica | 0:8c01a98a2812 | 110 | * #include "imu_driver.hpp" |
Arithemetica | 0:8c01a98a2812 | 111 | * |
Arithemetica | 0:8c01a98a2812 | 112 | * SPI spi3(PB_5, PB_6, PB_3); // declare SPI instance globally |
Arithemetica | 0:8c01a98a2812 | 113 | * Serial pc(USBTX, USBRX, 115200); // print debug message |
Arithemetica | 0:8c01a98a2812 | 114 | * |
Arithemetica | 0:8c01a98a2812 | 115 | * int main() |
Arithemetica | 0:8c01a98a2812 | 116 | * { |
Arithemetica | 5:e71931fcae33 | 117 | * // SPI instance, reset, data ready, slave select |
Arithemetica | 17:629b2f317d0a | 118 | * ImuDriver<spi3, PA_10, PA_8, PA_9> imu(ImuExtiRcvNormalMsg); |
Arithemetica | 0:8c01a98a2812 | 119 | * |
Arithemetica | 0:8c01a98a2812 | 120 | * while(true) |
Arithemetica | 0:8c01a98a2812 | 121 | * { |
Arithemetica | 17:629b2f317d0a | 122 | * pc.printf("%.3f, %.3f, %.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]); |
Arithemetica | 0:8c01a98a2812 | 123 | * } |
Arithemetica | 0:8c01a98a2812 | 124 | * } |
Arithemetica | 5:e71931fcae33 | 125 | * |
Arithemetica | 0:8c01a98a2812 | 126 | * @endcode |
Arithemetica | 0:8c01a98a2812 | 127 | */ |
Arithemetica | 2:f3a7def7a7e1 | 128 | template <SPI& Spi, PinName rst, PinName drdy, PinName ss> |
Arithemetica | 0:8c01a98a2812 | 129 | class ImuDriver |
Arithemetica | 0:8c01a98a2812 | 130 | { |
Arithemetica | 0:8c01a98a2812 | 131 | private: |
Arithemetica | 14:6c31a7ab45f0 | 132 | ImuExtiConfig m_extiConfig; |
Arithemetica | 14:6c31a7ab45f0 | 133 | |
Arithemetica | 0:8c01a98a2812 | 134 | SPI& m_spi; |
Arithemetica | 0:8c01a98a2812 | 135 | DigitalOut m_rst, m_ss; |
Arithemetica | 0:8c01a98a2812 | 136 | DigitalIn m_drdy; |
Arithemetica | 2:f3a7def7a7e1 | 137 | InterruptIn m_drdyExti; |
Arithemetica | 5:e71931fcae33 | 138 | |
Arithemetica | 0:8c01a98a2812 | 139 | float m_gyroScaler[3]; |
Arithemetica | 0:8c01a98a2812 | 140 | float m_accelScaler[3]; |
Arithemetica | 0:8c01a98a2812 | 141 | float m_ahrsScaler[3]; |
Arithemetica | 0:8c01a98a2812 | 142 | private: |
Arithemetica | 0:8c01a98a2812 | 143 | std::uint16_t m_imuSpiWrite(const std::uint16_t val) const |
Arithemetica | 0:8c01a98a2812 | 144 | { |
Arithemetica | 0:8c01a98a2812 | 145 | // RAII |
Arithemetica | 0:8c01a98a2812 | 146 | class SpiLock |
Arithemetica | 0:8c01a98a2812 | 147 | { |
Arithemetica | 0:8c01a98a2812 | 148 | private: |
Arithemetica | 0:8c01a98a2812 | 149 | SPI& m_spi; |
Arithemetica | 0:8c01a98a2812 | 150 | public: |
Arithemetica | 0:8c01a98a2812 | 151 | SpiLock(SPI& t_spi) : m_spi(t_spi) |
Arithemetica | 0:8c01a98a2812 | 152 | { |
Arithemetica | 0:8c01a98a2812 | 153 | m_spi.lock(); |
Arithemetica | 0:8c01a98a2812 | 154 | } |
Arithemetica | 0:8c01a98a2812 | 155 | |
Arithemetica | 0:8c01a98a2812 | 156 | ~SpiLock() |
Arithemetica | 0:8c01a98a2812 | 157 | { |
Arithemetica | 0:8c01a98a2812 | 158 | m_spi.unlock(); |
Arithemetica | 0:8c01a98a2812 | 159 | } |
Arithemetica | 0:8c01a98a2812 | 160 | }; |
Arithemetica | 0:8c01a98a2812 | 161 | |
Arithemetica | 0:8c01a98a2812 | 162 | SpiLock lock(m_spi); |
Arithemetica | 0:8c01a98a2812 | 163 | return m_spi.write(val); |
Arithemetica | 0:8c01a98a2812 | 164 | } |
Arithemetica | 0:8c01a98a2812 | 165 | |
Arithemetica | 0:8c01a98a2812 | 166 | inline std::uint16_t m_spiGenerateReadCmd(const std::uint8_t t_val) const |
Arithemetica | 0:8c01a98a2812 | 167 | { |
Arithemetica | 0:8c01a98a2812 | 168 | return t_val << 8U; |
Arithemetica | 0:8c01a98a2812 | 169 | } |
Arithemetica | 0:8c01a98a2812 | 170 | |
Arithemetica | 0:8c01a98a2812 | 171 | inline bool m_spiIsDataReady() |
Arithemetica | 0:8c01a98a2812 | 172 | { |
Arithemetica | 0:8c01a98a2812 | 173 | return m_drdy.read() == 0; |
Arithemetica | 0:8c01a98a2812 | 174 | } |
Arithemetica | 0:8c01a98a2812 | 175 | |
Arithemetica | 0:8c01a98a2812 | 176 | inline void m_processImuRawData() |
Arithemetica | 0:8c01a98a2812 | 177 | { |
Arithemetica | 0:8c01a98a2812 | 178 | for (int i = 0; i < 3; ++i) { |
Arithemetica | 0:8c01a98a2812 | 179 | imuProcessedData.accel[i] = imuRawData.accel[i] / m_accelScaler[i]; |
Arithemetica | 0:8c01a98a2812 | 180 | imuProcessedData.rate[i] = imuRawData.rate[i] / m_gyroScaler[i]; |
Arithemetica | 0:8c01a98a2812 | 181 | } |
Arithemetica | 0:8c01a98a2812 | 182 | } |
Arithemetica | 0:8c01a98a2812 | 183 | |
Arithemetica | 0:8c01a98a2812 | 184 | inline void m_processAhrsRawData() |
Arithemetica | 0:8c01a98a2812 | 185 | { |
Arithemetica | 0:8c01a98a2812 | 186 | for (int i = 0; i < 3; ++i) { |
Arithemetica | 0:8c01a98a2812 | 187 | ahrsProcessedData.attitude[i] = ahrsRawData.attitude[i] / m_ahrsScaler[i]; |
Arithemetica | 0:8c01a98a2812 | 188 | } |
Arithemetica | 0:8c01a98a2812 | 189 | } |
Arithemetica | 5:e71931fcae33 | 190 | |
Arithemetica | 14:6c31a7ab45f0 | 191 | void m_dataReadyExtiCallback() |
Arithemetica | 2:f3a7def7a7e1 | 192 | { |
Arithemetica | 14:6c31a7ab45f0 | 193 | if (m_extiConfig & ImuExtiRcvNormalMsg) { |
Arithemetica | 14:6c31a7ab45f0 | 194 | receiveBurstMsgImpl(); |
Arithemetica | 14:6c31a7ab45f0 | 195 | } |
Arithemetica | 15:ca49fdec90fc | 196 | |
Arithemetica | 14:6c31a7ab45f0 | 197 | if (m_extiConfig & ImuExtiRcvAhrsMsg) { |
Arithemetica | 16:6583ad08d0d0 | 198 | receiveAhrsMsgImpl(); |
Arithemetica | 14:6c31a7ab45f0 | 199 | } |
Arithemetica | 2:f3a7def7a7e1 | 200 | } |
Arithemetica | 14:6c31a7ab45f0 | 201 | private: |
Arithemetica | 16:6583ad08d0d0 | 202 | ImuDriver(); // cannot initialize via default constructor |
Arithemetica | 16:6583ad08d0d0 | 203 | |
Arithemetica | 14:6c31a7ab45f0 | 204 | ImuDriverStatus receiveBurstMsgImpl(bool t_extended = false) |
Arithemetica | 15:ca49fdec90fc | 205 | { |
Arithemetica | 0:8c01a98a2812 | 206 | if (m_spiIsDataReady()) { |
Arithemetica | 0:8c01a98a2812 | 207 | std::uint16_t max_data = 0U; |
Arithemetica | 0:8c01a98a2812 | 208 | std::uint16_t burst_reg = 0U; |
Arithemetica | 0:8c01a98a2812 | 209 | |
Arithemetica | 0:8c01a98a2812 | 210 | if (!t_extended) { |
Arithemetica | 0:8c01a98a2812 | 211 | max_data = 8U; |
Arithemetica | 0:8c01a98a2812 | 212 | burst_reg = m_spiGenerateReadCmd(ReadBurstDataRegister); |
Arithemetica | 0:8c01a98a2812 | 213 | } else { |
Arithemetica | 0:8c01a98a2812 | 214 | max_data = 11U; |
Arithemetica | 0:8c01a98a2812 | 215 | burst_reg = m_spiGenerateReadCmd(ReadExtBurstDataRegister); |
Arithemetica | 0:8c01a98a2812 | 216 | } |
Arithemetica | 0:8c01a98a2812 | 217 | |
Arithemetica | 0:8c01a98a2812 | 218 | m_ss.write(0); // start transfer |
Arithemetica | 0:8c01a98a2812 | 219 | m_imuSpiWrite(burst_reg); |
Arithemetica | 0:8c01a98a2812 | 220 | |
Arithemetica | 0:8c01a98a2812 | 221 | static std::uint8_t data_rcved = 0U; |
Arithemetica | 0:8c01a98a2812 | 222 | |
Arithemetica | 0:8c01a98a2812 | 223 | while(data_rcved < max_data) { |
Arithemetica | 0:8c01a98a2812 | 224 | const std::uint16_t spi_data = m_imuSpiWrite(0x0000); |
Arithemetica | 0:8c01a98a2812 | 225 | switch(data_rcved) { |
Arithemetica | 0:8c01a98a2812 | 226 | case 0: |
Arithemetica | 0:8c01a98a2812 | 227 | imuRawData.status = spi_data; |
Arithemetica | 0:8c01a98a2812 | 228 | break; |
Arithemetica | 0:8c01a98a2812 | 229 | case 1: |
Arithemetica | 0:8c01a98a2812 | 230 | case 2: |
Arithemetica | 0:8c01a98a2812 | 231 | case 3: |
Arithemetica | 0:8c01a98a2812 | 232 | imuRawData.rate[data_rcved - 1] = spi_data; |
Arithemetica | 0:8c01a98a2812 | 233 | break; |
Arithemetica | 0:8c01a98a2812 | 234 | case 4: |
Arithemetica | 0:8c01a98a2812 | 235 | case 5: |
Arithemetica | 0:8c01a98a2812 | 236 | case 6: |
Arithemetica | 0:8c01a98a2812 | 237 | imuRawData.accel[data_rcved - 4] = spi_data; |
Arithemetica | 0:8c01a98a2812 | 238 | break; |
Arithemetica | 0:8c01a98a2812 | 239 | case 7: |
Arithemetica | 0:8c01a98a2812 | 240 | imuRawData.temperature = spi_data; |
Arithemetica | 0:8c01a98a2812 | 241 | break; |
Arithemetica | 0:8c01a98a2812 | 242 | default: |
Arithemetica | 0:8c01a98a2812 | 243 | break; |
Arithemetica | 0:8c01a98a2812 | 244 | } |
Arithemetica | 0:8c01a98a2812 | 245 | |
Arithemetica | 0:8c01a98a2812 | 246 | ++data_rcved; |
Arithemetica | 0:8c01a98a2812 | 247 | } |
Arithemetica | 0:8c01a98a2812 | 248 | |
Arithemetica | 0:8c01a98a2812 | 249 | data_rcved = 0U; |
Arithemetica | 0:8c01a98a2812 | 250 | m_ss.write(1); |
Arithemetica | 0:8c01a98a2812 | 251 | |
Arithemetica | 0:8c01a98a2812 | 252 | m_processImuRawData(); |
Arithemetica | 0:8c01a98a2812 | 253 | return ImuDriverStatusOK; |
Arithemetica | 0:8c01a98a2812 | 254 | } else { |
Arithemetica | 0:8c01a98a2812 | 255 | return ImuDriverStatusDataNotReady; |
Arithemetica | 0:8c01a98a2812 | 256 | } |
Arithemetica | 0:8c01a98a2812 | 257 | } |
Arithemetica | 15:ca49fdec90fc | 258 | |
Arithemetica | 15:ca49fdec90fc | 259 | ImuDriverStatus receiveAhrsMsgImpl() |
Arithemetica | 15:ca49fdec90fc | 260 | { |
Arithemetica | 15:ca49fdec90fc | 261 | if (m_spiIsDataReady()) { |
Arithemetica | 15:ca49fdec90fc | 262 | static std::uint8_t data_rcved = 0U; |
Arithemetica | 15:ca49fdec90fc | 263 | const std::uint8_t max_data = 5U; |
Arithemetica | 15:ca49fdec90fc | 264 | const std::uint16_t ahrs_reg = m_spiGenerateReadCmd(ReadAhrsBurstDataRegister); |
Arithemetica | 15:ca49fdec90fc | 265 | |
Arithemetica | 15:ca49fdec90fc | 266 | m_ss.write(0); |
Arithemetica | 15:ca49fdec90fc | 267 | m_imuSpiWrite(ahrs_reg); |
Arithemetica | 15:ca49fdec90fc | 268 | |
Arithemetica | 15:ca49fdec90fc | 269 | while(data_rcved < max_data) { |
Arithemetica | 15:ca49fdec90fc | 270 | const std::uint16_t spi_data = m_imuSpiWrite(0x0000); |
Arithemetica | 15:ca49fdec90fc | 271 | switch(data_rcved) { |
Arithemetica | 15:ca49fdec90fc | 272 | case 0: |
Arithemetica | 15:ca49fdec90fc | 273 | ahrsRawData.status = spi_data; |
Arithemetica | 15:ca49fdec90fc | 274 | break; |
Arithemetica | 15:ca49fdec90fc | 275 | case 1: |
Arithemetica | 15:ca49fdec90fc | 276 | case 2: |
Arithemetica | 15:ca49fdec90fc | 277 | case 3: |
Arithemetica | 15:ca49fdec90fc | 278 | ahrsRawData.attitude[data_rcved - 1] = spi_data; |
Arithemetica | 15:ca49fdec90fc | 279 | break; |
Arithemetica | 15:ca49fdec90fc | 280 | case 4: |
Arithemetica | 15:ca49fdec90fc | 281 | ahrsRawData.temperature = spi_data; |
Arithemetica | 15:ca49fdec90fc | 282 | break; |
Arithemetica | 15:ca49fdec90fc | 283 | default: |
Arithemetica | 15:ca49fdec90fc | 284 | break; |
Arithemetica | 15:ca49fdec90fc | 285 | } |
Arithemetica | 15:ca49fdec90fc | 286 | |
Arithemetica | 15:ca49fdec90fc | 287 | ++data_rcved; |
Arithemetica | 17:629b2f317d0a | 288 | |
Arithemetica | 15:ca49fdec90fc | 289 | } |
Arithemetica | 15:ca49fdec90fc | 290 | |
Arithemetica | 15:ca49fdec90fc | 291 | data_rcved = 0U; |
Arithemetica | 15:ca49fdec90fc | 292 | m_ss.write(1); |
Arithemetica | 15:ca49fdec90fc | 293 | |
Arithemetica | 15:ca49fdec90fc | 294 | m_processAhrsRawData(); |
Arithemetica | 15:ca49fdec90fc | 295 | return ImuDriverStatusOK; |
Arithemetica | 15:ca49fdec90fc | 296 | } else { |
Arithemetica | 15:ca49fdec90fc | 297 | return ImuDriverStatusDataNotReady; |
Arithemetica | 15:ca49fdec90fc | 298 | } |
Arithemetica | 15:ca49fdec90fc | 299 | } |
Arithemetica | 14:6c31a7ab45f0 | 300 | public: |
Arithemetica | 14:6c31a7ab45f0 | 301 | /** |
Arithemetica | 14:6c31a7ab45f0 | 302 | * imuRawData contains raw data received from @ref receiveBurstMsg(bool t_extended) |
Arithemetica | 14:6c31a7ab45f0 | 303 | */ |
Arithemetica | 14:6c31a7ab45f0 | 304 | ImuRawData imuRawData; |
Arithemetica | 14:6c31a7ab45f0 | 305 | |
Arithemetica | 14:6c31a7ab45f0 | 306 | /** |
Arithemetica | 14:6c31a7ab45f0 | 307 | * imuProcessedData contains processed data, which is merely scaling operation |
Arithemetica | 14:6c31a7ab45f0 | 308 | */ |
Arithemetica | 14:6c31a7ab45f0 | 309 | ImuProcessedData imuProcessedData; |
Arithemetica | 14:6c31a7ab45f0 | 310 | |
Arithemetica | 14:6c31a7ab45f0 | 311 | /** |
Arithemetica | 14:6c31a7ab45f0 | 312 | * ahrsRawaData contains raw data received from @ref receiveAhrsMsg() |
Arithemetica | 14:6c31a7ab45f0 | 313 | */ |
Arithemetica | 14:6c31a7ab45f0 | 314 | AhrsRawData ahrsRawData; |
Arithemetica | 14:6c31a7ab45f0 | 315 | |
Arithemetica | 14:6c31a7ab45f0 | 316 | /** |
Arithemetica | 14:6c31a7ab45f0 | 317 | * ahrsProcessedData contains processed data, which is merely scaling operation |
Arithemetica | 14:6c31a7ab45f0 | 318 | */ |
Arithemetica | 14:6c31a7ab45f0 | 319 | AhrsProcessedData ahrsProcessedData; |
Arithemetica | 14:6c31a7ab45f0 | 320 | |
Arithemetica | 14:6c31a7ab45f0 | 321 | static const int DEFAULT_IMU_ACCEL_SCALER = 4000; |
Arithemetica | 14:6c31a7ab45f0 | 322 | static const int DEFAULT_IMU_GYRO_SCALER = 100; |
Arithemetica | 14:6c31a7ab45f0 | 323 | static const int DEFAULT_AHRS_ATTITUDE_SCALER = 90; |
Arithemetica | 14:6c31a7ab45f0 | 324 | public: |
Arithemetica | 14:6c31a7ab45f0 | 325 | 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) |
Arithemetica | 14:6c31a7ab45f0 | 326 | { |
Arithemetica | 14:6c31a7ab45f0 | 327 | for (int i = 0; i < 3; ++i) { |
Arithemetica | 14:6c31a7ab45f0 | 328 | m_gyroScaler[i] = static_cast<float>(DEFAULT_IMU_GYRO_SCALER); // what an idiotic way of initialization LUL |
Arithemetica | 14:6c31a7ab45f0 | 329 | m_accelScaler[i] = static_cast<float>(DEFAULT_IMU_ACCEL_SCALER); // Oh, I forgot that mbed os2 still stucks at c++98, |
Arithemetica | 14:6c31a7ab45f0 | 330 | m_ahrsScaler[i] = static_cast<float>(DEFAULT_AHRS_ATTITUDE_SCALER); // how unfortunate LUL |
Arithemetica | 14:6c31a7ab45f0 | 331 | } |
Arithemetica | 14:6c31a7ab45f0 | 332 | |
Arithemetica | 14:6c31a7ab45f0 | 333 | if (t_exti_config != ImuExtiRcvNotUsed) { |
Arithemetica | 14:6c31a7ab45f0 | 334 | m_drdyExti.fall(this, &ImuDriver<Spi, rst, drdy, ss>::m_dataReadyExtiCallback); |
Arithemetica | 17:629b2f317d0a | 335 | m_drdyExti.enable_irq(); |
Arithemetica | 14:6c31a7ab45f0 | 336 | } |
Arithemetica | 14:6c31a7ab45f0 | 337 | |
Arithemetica | 14:6c31a7ab45f0 | 338 | m_spi.format(16, 3); |
Arithemetica | 17:629b2f317d0a | 339 | m_spi.frequency(); |
Arithemetica | 14:6c31a7ab45f0 | 340 | } |
Arithemetica | 14:6c31a7ab45f0 | 341 | |
Arithemetica | 14:6c31a7ab45f0 | 342 | /** |
Arithemetica | 14:6c31a7ab45f0 | 343 | * @brief This function handles the receiving of burst message function, burst message |
Arithemetica | 14:6c31a7ab45f0 | 344 | * contains accelerometer (g), gyroscope (deg/s), status and temperature data (Celcius) |
Arithemetica | 14:6c31a7ab45f0 | 345 | * |
Arithemetica | 14:6c31a7ab45f0 | 346 | * @param t_extended bool to indicate the message type is extended or not, default false |
Arithemetica | 14:6c31a7ab45f0 | 347 | * |
Arithemetica | 14:6c31a7ab45f0 | 348 | * @return ImuDriverStatus to indicate the result of the receive operation |
Arithemetica | 14:6c31a7ab45f0 | 349 | */ |
Arithemetica | 15:ca49fdec90fc | 350 | ImuDriverStatus receiveBurstMsg(bool t_extended = false) |
Arithemetica | 15:ca49fdec90fc | 351 | { |
Arithemetica | 15:ca49fdec90fc | 352 | return (m_extiConfig & ImuExtiRcvNormalMsg ? ImuDriverStatusInvalidCall : receiveBurstMsgImpl(t_extended)); |
Arithemetica | 14:6c31a7ab45f0 | 353 | } |
Arithemetica | 0:8c01a98a2812 | 354 | |
Arithemetica | 0:8c01a98a2812 | 355 | /** |
Arithemetica | 5:e71931fcae33 | 356 | * @brief This function handles the receiving of AHRS burst message function, AHRS |
Arithemetica | 10:d8223969c541 | 357 | * burst message contains the attitude in euler angle (deg). |
Arithemetica | 0:8c01a98a2812 | 358 | * |
Arithemetica | 0:8c01a98a2812 | 359 | * @return ImuDriverStatus to indicate the result of the receive operation |
Arithemetica | 0:8c01a98a2812 | 360 | * |
Arithemetica | 0:8c01a98a2812 | 361 | * @note This is only suitable for AHRS app, and additional procedure needs to be done |
Arithemetica | 0:8c01a98a2812 | 362 | * in order to make it work |
Arithemetica | 0:8c01a98a2812 | 363 | */ |
Arithemetica | 0:8c01a98a2812 | 364 | ImuDriverStatus receiveAhrsMsg() |
Arithemetica | 0:8c01a98a2812 | 365 | { |
Arithemetica | 15:ca49fdec90fc | 366 | return (m_extiConfig & ImuExtiRcvAhrsMsg ? ImuDriverStatusInvalidCall : receiveAhrsMsgImpl()); |
Arithemetica | 15:ca49fdec90fc | 367 | } |
Arithemetica | 0:8c01a98a2812 | 368 | |
Arithemetica | 17:629b2f317d0a | 369 | /** |
Arithemetica | 17:629b2f317d0a | 370 | * @brief This function handles the external interrupt option, whether receive AHRS |
Arithemetica | 17:629b2f317d0a | 371 | * burst message or normal imu message. |
Arithemetica | 17:629b2f317d0a | 372 | * |
Arithemetica | 17:629b2f317d0a | 373 | * @return ImuDriverStatus to indicate the result of the receive operation |
Arithemetica | 17:629b2f317d0a | 374 | * |
Arithemetica | 17:629b2f317d0a | 375 | * @note Never try receiving two messages at the same time |
Arithemetica | 17:629b2f317d0a | 376 | */ |
Arithemetica | 17:629b2f317d0a | 377 | ImuDriverStatus configExti(const ImuExtiConfig t_exti_config) const |
Arithemetica | 15:ca49fdec90fc | 378 | { |
Arithemetica | 14:6c31a7ab45f0 | 379 | m_extiConfig = t_exti_config; |
Arithemetica | 17:629b2f317d0a | 380 | |
Arithemetica | 17:629b2f317d0a | 381 | if (m_extoConfig == ImuExtiRcvNotUsed) { |
Arithemetica | 17:629b2f317d0a | 382 | m_drdyExti.disable_irq(); |
Arithemetica | 17:629b2f317d0a | 383 | } else { |
Arithemetica | 17:629b2f317d0a | 384 | m_drdyExti.enable_irq(); |
Arithemetica | 17:629b2f317d0a | 385 | } |
Arithemetica | 17:629b2f317d0a | 386 | |
Arithemetica | 17:629b2f317d0a | 387 | return ImuDriverStatusOK; |
Arithemetica | 14:6c31a7ab45f0 | 388 | } |
Arithemetica | 1:8e31413068af | 389 | }; |
Arithemetica | 1:8e31413068af | 390 | |
Arithemetica | 1:8e31413068af | 391 | #endif // IMU_DRIVER_HPP_ |