Diff: MEMS_Sensor_wrapper.hpp

Revision:

91:1dd4cdaede06

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MEMS_Sensor_wrapper.hpp	Sun Oct 16 05:40:03 2016 +0000
@@ -0,0 +1,194 @@
+/* Define to prevent from recursive inclusion --------------------------------*/
+#ifndef MEMS_SENSOR_WRAPPER_H
+#define MEMS_SENSOR_WRAPPER_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "mbed.h"
+#include "x_nucleo_iks01a1.h"
+
+/* Macros/typedefs -----------------------------------------------------------*/
+#define DEGREES                 "\u00B0"
+#define PI                      3.14159265
+
+#define RAD2DEG(X)              ((X * 180) / PI)
+
+#define DATA2VECTOR(D, T)       (vector<T>) { D[0], D[1], D[2] }
+
+#define min(X, Y)               (X < Y ? X : Y)
+#define max(X, Y)               (X > Y ? X : Y)
+
+#define vector_minimize(V, M)   V.x = min(V.x, M.x); V.y = min(V.y, M.y); V.z = min(V.z, M.z)
+#define vector_maximize(V, M)   V.x = max(V.x, M.x); V.y = max(V.y, M.y); V.z = max(V.z, M.z)
+
+template <typename T> 
+struct vector {
+  T x, y, z;
+};
+
+typedef struct {
+    float temp_1, temp_2;
+    float humidity, pressure;
+    float heading;
+    vector<int32_t> mag, acc, gyro;
+    vector<int32_t> mag_avg;
+    bool mag_isCalibrated;
+    vector<int16_t> defaultHeading;
+} SensorData;
+
+/* Variable definitions ------------------------------------------------------*/
+/* Instantiate the expansion board */
+static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
+
+/* Retrieve the composing elements of the expansion board */
+static GyroSensor       *gyroscope          = mems_expansion_board->GetGyroscope();
+static MotionSensor     *accelerometer      = mems_expansion_board->GetAccelerometer();
+static MagneticSensor   *magnetometer       = mems_expansion_board->magnetometer;
+static HumiditySensor   *humidity_sensor    = mems_expansion_board->ht_sensor;
+static PressureSensor   *pressure_sensor    = mems_expansion_board->pt_sensor;
+static TempSensor       *temp_sensor1       = mems_expansion_board->ht_sensor;
+static TempSensor       *temp_sensor2       = mems_expansion_board->pt_sensor;
+
+DigitalOut              status_led(LED1);
+
+static SensorData       MEMS_Data = { 0.0, 0.0,
+                                      0.0, 0.0,
+                                      0.0,
+                                      { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 },
+                                      { 0, 0, 0 },
+                                      false,
+                                      { 1, 0, 0}
+                                    };
+
+static float            rawFloat            = 0.0;
+static int32_t          rawVector[3]        = { 0, 0, 0};
+
+/* Code ----------------------------------------------------------------------*/
+// HTS221
+#define getTemp_1(V)            temp_sensor1->GetTemperature(&rawFloat); V = rawFloat
+#define getTemp_1_Fahr(V)       temp_sensor1->GetFahrenheit(&rawFloat); V = rawFloat
+#define getHumidity(V)          humidity_sensor->GetHumidity(&rawFloat); V = rawFloat
+
+// LPS25H
+#define getTemp_2(V)            temp_sensor2->GetTemperature(&rawFloat); V = rawFloat
+#define getTemp_2_Fahr(V)       temp_sensor2->GetFahrenheit(&rawFloat); V = rawFloat
+#define getPressure(V)          pressure_sensor->GetPressure(&rawFloat); V = rawFloat
+
+// LIS3MDL
+#define getMagnetoVector(V)     magnetometer->Get_M_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
+
+// LSM6DS0
+#define getAcceleroVector(V)    accelerometer->Get_X_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
+#define getGyroVector(V)        gyroscope->Get_G_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
+
+template <typename Ta, typename Tb, typename To> 
+void vector_cross(const vector<Ta>& a, const vector<Tb>& b, vector<To>& out){
+    out.x = (a.y * b.z) - (a.z * b.y);
+    out.y = (a.z * b.x) - (a.x * b.z);
+    out.z = (a.x * b.y) - (a.y * b.x);
+}
+
+template <typename Ta, typename Tb> 
+float vector_dot(const vector<Ta>& a, const vector<Tb>& b) {
+    return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
+}
+
+void vector_normalize(vector<float>& a) {
+    float mag = sqrt(vector_dot(a, a));
+    a.x /= mag;
+    a.y /= mag;
+    a.z /= mag;
+}
+
+void printMEMS_info(void) {
+    uint8_t id;
+    
+    humidity_sensor->ReadID(&id);
+    printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
+    pressure_sensor->ReadID(&id);
+    printf("LPS25H  pressure & temperature    = 0x%X\r\n", id);
+    magnetometer->ReadID(&id);
+    printf("LIS3MDL magnetometer              = 0x%X\r\n", id);
+    gyroscope->ReadID(&id);
+    printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);   
+}
+
+void calibrateMagnetometer(void) {
+    if (!MEMS_Data.mag_isCalibrated) {
+        int16_t i;
+        vector<int32_t> rawvect,
+                        mag_min = {+32767, +32767, +32767},
+                        mag_max = {-32767, -32767, -32767};
+        
+        printf("Calibrating Magnetometer: Move MEMS board in figure 8 while rotating in place for ~10 seconds...\r\n");
+        
+        for (i = 0; i < 9750; ++i) {
+            getMagnetoVector(rawvect);
+            
+            vector_minimize(mag_min, rawvect);
+            vector_maximize(mag_max, rawvect);
+            
+            if (i % 50 == 0) status_led = !status_led.read();
+            wait_ms(1);
+        }
+        
+        MEMS_Data.mag_avg = (vector<int32_t>) { ((mag_min.x + mag_max.x) / 2),
+                                                ((mag_min.y + mag_max.y) / 2),
+                                                ((mag_min.z + mag_max.z) / 2)
+                                              };
+        status_led = 1;
+        printf("Calibrating Magnetometer done.\r\n");
+        MEMS_Data.mag_isCalibrated = true;
+    } else
+        printf("Magnetometer is calibrated.\r\n");
+}
+
+/*
+ *  Default heading: (vector<int>) {1, 0, 0}
+ *  
+ *  getMagnetoVector(V) and getAcceleroVector(V) should by called prior to this.
+ */
+void getCompassheading(float& heading) {
+    // subtract offset (average of min and max) from magnetometer readings
+    vector<int> temp_m = {  MEMS_Data.mag.x - MEMS_Data.mag_avg.x, 
+                            MEMS_Data.mag.y - MEMS_Data.mag_avg.y, 
+                            MEMS_Data.mag.z - MEMS_Data.mag_avg.z
+                         };
+    // compute E and N
+    vector<float> E, N;
+    vector_cross(temp_m, MEMS_Data.acc, E);
+    vector_normalize(E);
+    vector_cross(MEMS_Data.acc, E, N);
+    vector_normalize(N);
+
+    // compute heading
+    heading = RAD2DEG(atan2((double)vector_dot(E, MEMS_Data.defaultHeading), (double)vector_dot(N, MEMS_Data.defaultHeading)));
+    if (heading < 0) heading += 360;
+}
+
+void updateSensorData(void) {  
+    if (!MEMS_Data.mag_isCalibrated)
+        calibrateMagnetometer();
+        
+    getTemp_1(MEMS_Data.temp_1);
+    getHumidity(MEMS_Data.humidity);
+    
+    getTemp_2(MEMS_Data.temp_2);
+    getPressure(MEMS_Data.pressure);
+
+    getMagnetoVector(MEMS_Data.mag);
+    getAcceleroVector(MEMS_Data.acc);
+    getGyroVector(MEMS_Data.gyro);
+    
+    getCompassheading(MEMS_Data.heading);
+}
+
+void printSensorData(void) {
+    printf("HTS221: [temp] % 3.2f%sC,   [hum]  % 3.2f%%\r\n", MEMS_Data.temp_1, DEGREES, MEMS_Data.humidity);
+    printf("LPS25H: [temp] % 3.2f%sC, [press] %4.2f mbar\r\n", MEMS_Data.temp_2, DEGREES, MEMS_Data.pressure);
+    printf("LIS3MDL [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", MEMS_Data.mag.x, MEMS_Data.mag.y, MEMS_Data.mag.z);
+    printf("LSM6DS0 [acc/mg]:      %6ld, %6ld, %6ld\r\n", MEMS_Data.acc.x, MEMS_Data.acc.y, MEMS_Data.acc.z);    
+    printf("        [Heading]:     %3.2f%s\r\n", MEMS_Data.heading, DEGREES);
+    printf("LSM6DS0 [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", MEMS_Data.gyro.x, MEMS_Data.gyro.y, MEMS_Data.gyro.z);
+}
+
+#endif /* MEMS_SENSOR_WRAPPER_H */