BA / Mbed OS BaBoRo_test2

Backup 1

IMU.h@0:02dd72d1d465, 2018-04-24 (annotated)

Committer:
borlanic
Date:
Tue Apr 24 11:45:18 2018 +0000
Revision:
0:02dd72d1d465
BaBoRo_test2 - backup 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
borlanic 0:02dd72d1d465 1 /*
borlanic 0:02dd72d1d465 2 * IMU.h
borlanic 0:02dd72d1d465 3 * Copyright (c) 2018, ZHAW
borlanic 0:02dd72d1d465 4 * All rights reserved.
borlanic 0:02dd72d1d465 5 */
borlanic 0:02dd72d1d465 6
borlanic 0:02dd72d1d465 7 #ifndef IMU_H_
borlanic 0:02dd72d1d465 8 #define IMU_H_
borlanic 0:02dd72d1d465 9
borlanic 0:02dd72d1d465 10 #include <cstdlib>
borlanic 0:02dd72d1d465 11 #include <stdint.h>
borlanic 0:02dd72d1d465 12 #include "mbed.h"
borlanic 0:02dd72d1d465 13 #include "LowpassFilter.h"
borlanic 0:02dd72d1d465 14
borlanic 0:02dd72d1d465 15
borlanic 0:02dd72d1d465 16 /**
borlanic 0:02dd72d1d465 17 * This is a device driver class for the ST LSM9DS1 inertial measurement unit.
borlanic 0:02dd72d1d465 18 */
borlanic 0:02dd72d1d465 19 class IMU {
borlanic 0:02dd72d1d465 20
borlanic 0:02dd72d1d465 21 public:
borlanic 0:02dd72d1d465 22
borlanic 0:02dd72d1d465 23 IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM);
borlanic 0:02dd72d1d465 24 virtual ~IMU();
borlanic 0:02dd72d1d465 25 float readGyroX();
borlanic 0:02dd72d1d465 26 float readGyroY();
borlanic 0:02dd72d1d465 27 float readGyroZ();
borlanic 0:02dd72d1d465 28 float readAccelerationX();
borlanic 0:02dd72d1d465 29 float readAccelerationY();
borlanic 0:02dd72d1d465 30 float readAccelerationZ();
borlanic 0:02dd72d1d465 31 float readMagnetometerX();
borlanic 0:02dd72d1d465 32 float readMagnetometerY();
borlanic 0:02dd72d1d465 33 float readMagnetometerZ();
borlanic 0:02dd72d1d465 34 // Nick =================
borlanic 0:02dd72d1d465 35 float getGammaX();
borlanic 0:02dd72d1d465 36 float getGammaY();
borlanic 0:02dd72d1d465 37 float getGammaZ();
borlanic 0:02dd72d1d465 38 float getDGammaX();
borlanic 0:02dd72d1d465 39 float getDGammaY();
borlanic 0:02dd72d1d465 40 float getDGammaZ();
borlanic 0:02dd72d1d465 41 void kalman();
borlanic 0:02dd72d1d465 42 //=======================
borlanic 0:02dd72d1d465 43
borlanic 0:02dd72d1d465 44 private:
borlanic 0:02dd72d1d465 45
borlanic 0:02dd72d1d465 46 static const uint8_t WHO_AM_I = 0x0F;
borlanic 0:02dd72d1d465 47 static const uint8_t CTRL_REG1_G = 0x10;
borlanic 0:02dd72d1d465 48 static const uint8_t CTRL_REG2_G = 0x11;
borlanic 0:02dd72d1d465 49 static const uint8_t CTRL_REG3_G = 0x12;
borlanic 0:02dd72d1d465 50 static const uint8_t OUT_X_L_G = 0x18;
borlanic 0:02dd72d1d465 51 static const uint8_t OUT_X_H_G = 0x19;
borlanic 0:02dd72d1d465 52 static const uint8_t OUT_Y_L_G = 0x1A;
borlanic 0:02dd72d1d465 53 static const uint8_t OUT_Y_H_G = 0x1B;
borlanic 0:02dd72d1d465 54 static const uint8_t OUT_Z_L_G = 0x1C;
borlanic 0:02dd72d1d465 55 static const uint8_t OUT_Z_H_G = 0x1D;
borlanic 0:02dd72d1d465 56 static const uint8_t CTRL_REG4 = 0x1E;
borlanic 0:02dd72d1d465 57 static const uint8_t CTRL_REG5_XL = 0x1F;
borlanic 0:02dd72d1d465 58 static const uint8_t CTRL_REG6_XL = 0x20;
borlanic 0:02dd72d1d465 59 static const uint8_t CTRL_REG7_XL = 0x21;
borlanic 0:02dd72d1d465 60 static const uint8_t CTRL_REG8 = 0x22;
borlanic 0:02dd72d1d465 61 static const uint8_t CTRL_REG9 = 0x23;
borlanic 0:02dd72d1d465 62 static const uint8_t CTRL_REG10 = 0x24;
borlanic 0:02dd72d1d465 63 static const uint8_t OUT_X_L_XL = 0x28;
borlanic 0:02dd72d1d465 64 static const uint8_t OUT_X_H_XL = 0x29;
borlanic 0:02dd72d1d465 65 static const uint8_t OUT_Y_L_XL = 0x2A;
borlanic 0:02dd72d1d465 66 static const uint8_t OUT_Y_H_XL = 0x2B;
borlanic 0:02dd72d1d465 67 static const uint8_t OUT_Z_L_XL = 0x2C;
borlanic 0:02dd72d1d465 68 static const uint8_t OUT_Z_H_XL = 0x2D;
borlanic 0:02dd72d1d465 69
borlanic 0:02dd72d1d465 70 static const uint8_t WHO_AM_I_M = 0x0F;
borlanic 0:02dd72d1d465 71 static const uint8_t CTRL_REG1_M = 0x20;
borlanic 0:02dd72d1d465 72 static const uint8_t CTRL_REG2_M = 0x21;
borlanic 0:02dd72d1d465 73 static const uint8_t CTRL_REG3_M = 0x22;
borlanic 0:02dd72d1d465 74 static const uint8_t CTRL_REG4_M = 0x23;
borlanic 0:02dd72d1d465 75 static const uint8_t CTRL_REG5_M = 0x24;
borlanic 0:02dd72d1d465 76 static const uint8_t OUT_X_L_M = 0x28;
borlanic 0:02dd72d1d465 77 static const uint8_t OUT_X_H_M = 0x29;
borlanic 0:02dd72d1d465 78 static const uint8_t OUT_Y_L_M = 0x2A;
borlanic 0:02dd72d1d465 79 static const uint8_t OUT_Y_H_M = 0x2B;
borlanic 0:02dd72d1d465 80 static const uint8_t OUT_Z_L_M = 0x2C;
borlanic 0:02dd72d1d465 81 static const uint8_t OUT_Z_H_M = 0x2D;
borlanic 0:02dd72d1d465 82
borlanic 0:02dd72d1d465 83 static const float M_PI;
borlanic 0:02dd72d1d465 84 static const float SAMPLE_TIME;
borlanic 0:02dd72d1d465 85 static const float STD_ALPHA;
borlanic 0:02dd72d1d465 86 static const float STD_OMEGA;
borlanic 0:02dd72d1d465 87 static const uint32_t STACK_SIZE = 4096; // stack size of thread, given in [bytes]
borlanic 0:02dd72d1d465 88
borlanic 0:02dd72d1d465 89 float gammaX; // angle in X from kalman-Filter
borlanic 0:02dd72d1d465 90 float gammaY; // angle in Y from kalman-Filter
borlanic 0:02dd72d1d465 91 float gammaZ; // angle in Z from kalman-Filter
borlanic 0:02dd72d1d465 92 float d_gammaX; // angular velocity in X from kalman-Filter
borlanic 0:02dd72d1d465 93 float d_gammaY; // angular velocity in Y from kalman-Filter
borlanic 0:02dd72d1d465 94 float d_gammaZ; // angular velocity in Z from kalman-Filter
borlanic 0:02dd72d1d465 95
borlanic 0:02dd72d1d465 96 LowpassFilter gammaXFilter;
borlanic 0:02dd72d1d465 97 LowpassFilter gammaYFilter;
borlanic 0:02dd72d1d465 98 LowpassFilter d_gammaXFilter;
borlanic 0:02dd72d1d465 99 LowpassFilter d_gammaYFilter;
borlanic 0:02dd72d1d465 100
borlanic 0:02dd72d1d465 101 Thread thread;
borlanic 0:02dd72d1d465 102 Ticker ticker;
borlanic 0:02dd72d1d465 103 Mutex mutex; // mutex to lock critical sections
borlanic 0:02dd72d1d465 104
borlanic 0:02dd72d1d465 105
borlanic 0:02dd72d1d465 106 SPI& spi;
borlanic 0:02dd72d1d465 107 DigitalOut& csAG;
borlanic 0:02dd72d1d465 108 DigitalOut& csM;
borlanic 0:02dd72d1d465 109
borlanic 0:02dd72d1d465 110 void writeRegister(DigitalOut& cs, uint8_t address, uint8_t value);
borlanic 0:02dd72d1d465 111 uint8_t readRegister(DigitalOut& cs, uint8_t address);
borlanic 0:02dd72d1d465 112 };
borlanic 0:02dd72d1d465 113
borlanic 0:02dd72d1d465 114 #endif /* IMU_H_ */
borlanic 0:02dd72d1d465 115