Nicolas Borla
BBR 1 Ebene
Diff: IMU.h
- Revision:
- 0:fbdae7e6d805
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h Mon May 14 11:29:06 2018 +0000
@@ -0,0 +1,120 @@
+/*
+ * IMU.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <cstdlib>
+#include <stdint.h>
+#include "mbed.h"
+#include "LowpassFilter.h"
+#include "Signal.h"
+#include "SerialCom.h"
+
+
+/**
+ * This is a device driver class for the ST LSM9DS1 inertial measurement unit.
+ */
+class IMU {
+
+ public:
+
+ IMU(SPI& spi, DigitalOut& csAG, DigitalOut& csM);
+ virtual ~IMU();
+ float readGyroX();
+ float readGyroY();
+ float readGyroZ();
+ float readAccelerationX();
+ float readAccelerationY();
+ float readAccelerationZ();
+ float readMagnetometerX();
+ float readMagnetometerY();
+ float readMagnetometerZ();
+ // Nick =================
+ float getGammaX();
+ float getGammaY();
+ float getGammaZ();
+ float getDGammaX();
+ float getDGammaY();
+ float getDGammaZ();
+
+ private:
+
+ static const uint8_t WHO_AM_I = 0x0F;
+ static const uint8_t CTRL_REG1_G = 0x10;
+ static const uint8_t CTRL_REG2_G = 0x11;
+ static const uint8_t CTRL_REG3_G = 0x12;
+ static const uint8_t OUT_X_L_G = 0x18;
+ static const uint8_t OUT_X_H_G = 0x19;
+ static const uint8_t OUT_Y_L_G = 0x1A;
+ static const uint8_t OUT_Y_H_G = 0x1B;
+ static const uint8_t OUT_Z_L_G = 0x1C;
+ static const uint8_t OUT_Z_H_G = 0x1D;
+ static const uint8_t CTRL_REG4 = 0x1E;
+ static const uint8_t CTRL_REG5_XL = 0x1F;
+ static const uint8_t CTRL_REG6_XL = 0x20;
+ static const uint8_t CTRL_REG7_XL = 0x21;
+ static const uint8_t CTRL_REG8 = 0x22;
+ static const uint8_t CTRL_REG9 = 0x23;
+ static const uint8_t CTRL_REG10 = 0x24;
+ static const uint8_t OUT_X_L_XL = 0x28;
+ static const uint8_t OUT_X_H_XL = 0x29;
+ static const uint8_t OUT_Y_L_XL = 0x2A;
+ static const uint8_t OUT_Y_H_XL = 0x2B;
+ static const uint8_t OUT_Z_L_XL = 0x2C;
+ static const uint8_t OUT_Z_H_XL = 0x2D;
+
+ static const uint8_t WHO_AM_I_M = 0x0F;
+ static const uint8_t CTRL_REG1_M = 0x20;
+ static const uint8_t CTRL_REG2_M = 0x21;
+ static const uint8_t CTRL_REG3_M = 0x22;
+ static const uint8_t CTRL_REG4_M = 0x23;
+ static const uint8_t CTRL_REG5_M = 0x24;
+ static const uint8_t OUT_X_L_M = 0x28;
+ static const uint8_t OUT_X_H_M = 0x29;
+ static const uint8_t OUT_Y_L_M = 0x2A;
+ static const uint8_t OUT_Y_H_M = 0x2B;
+ static const uint8_t OUT_Z_L_M = 0x2C;
+ static const uint8_t OUT_Z_H_M = 0x2D;
+
+ static const float M_PI;
+ static const float SAMPLE_TIME;
+ static const float STD_ALPHA;
+ static const float STD_OMEGA;
+ static const uint32_t STACK_SIZE = 4096; // stack size of thread, given in [bytes]
+
+ float gammaX; // angle in X from kalman-Filter
+ float gammaY; // angle in Y from kalman-Filter
+ float gammaZ; // angle in Z from kalman-Filter
+ float d_gammaX; // angular velocity in X from kalman-Filter
+ float d_gammaY; // angular velocity in Y from kalman-Filter
+ float d_gammaZ; // angular velocity in Z from kalman-Filter
+
+ LowpassFilter gammaXFilter;
+ LowpassFilter gammaYFilter;
+ LowpassFilter d_gammaXFilter;
+ LowpassFilter d_gammaYFilter;
+
+ Signal signal;
+ Thread thread;
+ Ticker ticker;
+ Mutex mutex; // mutex to lock critical sections
+
+ void sendSignal();
+ void kalman();
+
+ SPI& spi;
+ DigitalOut& csAG;
+ DigitalOut& csM;
+
+ void writeRegister(DigitalOut& cs, uint8_t address, uint8_t value);
+ uint8_t readRegister(DigitalOut& cs, uint8_t address);
+};
+
+#endif /* IMU_H_ */
+
+
+