William Thenaers / X_NUCLEO_IKS01A1

Read and store X_NUCLEO_IKS01A1 data. Also calculates compass heading.

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Dependents:   HelloWorld_IKS01A1

Fork of X_NUCLEO_IKS01A1 by ST

MEMS_Sensor_wrapper.hpp@91:1dd4cdaede06, 2016-10-16 (annotated)

Committer:
Wosser1sProductions
Date:
Sun Oct 16 05:40:03 2016 +0000
Revision:
91:1dd4cdaede06
Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wosser1sProductions 91:1dd4cdaede06 1 /* Define to prevent from recursive inclusion --------------------------------*/
Wosser1sProductions 91:1dd4cdaede06 2 #ifndef MEMS_SENSOR_WRAPPER_H
Wosser1sProductions 91:1dd4cdaede06 3 #define MEMS_SENSOR_WRAPPER_H
Wosser1sProductions 91:1dd4cdaede06 4
Wosser1sProductions 91:1dd4cdaede06 5 /* Includes ------------------------------------------------------------------*/
Wosser1sProductions 91:1dd4cdaede06 6 #include "mbed.h"
Wosser1sProductions 91:1dd4cdaede06 7 #include "x_nucleo_iks01a1.h"
Wosser1sProductions 91:1dd4cdaede06 8
Wosser1sProductions 91:1dd4cdaede06 9 /* Macros/typedefs -----------------------------------------------------------*/
Wosser1sProductions 91:1dd4cdaede06 10 #define DEGREES "\u00B0"
Wosser1sProductions 91:1dd4cdaede06 11 #define PI 3.14159265
Wosser1sProductions 91:1dd4cdaede06 12
Wosser1sProductions 91:1dd4cdaede06 13 #define RAD2DEG(X) ((X * 180) / PI)
Wosser1sProductions 91:1dd4cdaede06 14
Wosser1sProductions 91:1dd4cdaede06 15 #define DATA2VECTOR(D, T) (vector<T>) { D[0], D[1], D[2] }
Wosser1sProductions 91:1dd4cdaede06 16
Wosser1sProductions 91:1dd4cdaede06 17 #define min(X, Y) (X < Y ? X : Y)
Wosser1sProductions 91:1dd4cdaede06 18 #define max(X, Y) (X > Y ? X : Y)
Wosser1sProductions 91:1dd4cdaede06 19
Wosser1sProductions 91:1dd4cdaede06 20 #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)
Wosser1sProductions 91:1dd4cdaede06 21 #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)
Wosser1sProductions 91:1dd4cdaede06 22
Wosser1sProductions 91:1dd4cdaede06 23 template <typename T>
Wosser1sProductions 91:1dd4cdaede06 24 struct vector {
Wosser1sProductions 91:1dd4cdaede06 25 T x, y, z;
Wosser1sProductions 91:1dd4cdaede06 26 };
Wosser1sProductions 91:1dd4cdaede06 27
Wosser1sProductions 91:1dd4cdaede06 28 typedef struct {
Wosser1sProductions 91:1dd4cdaede06 29 float temp_1, temp_2;
Wosser1sProductions 91:1dd4cdaede06 30 float humidity, pressure;
Wosser1sProductions 91:1dd4cdaede06 31 float heading;
Wosser1sProductions 91:1dd4cdaede06 32 vector<int32_t> mag, acc, gyro;
Wosser1sProductions 91:1dd4cdaede06 33 vector<int32_t> mag_avg;
Wosser1sProductions 91:1dd4cdaede06 34 bool mag_isCalibrated;
Wosser1sProductions 91:1dd4cdaede06 35 vector<int16_t> defaultHeading;
Wosser1sProductions 91:1dd4cdaede06 36 } SensorData;
Wosser1sProductions 91:1dd4cdaede06 37
Wosser1sProductions 91:1dd4cdaede06 38 /* Variable definitions ------------------------------------------------------*/
Wosser1sProductions 91:1dd4cdaede06 39 /* Instantiate the expansion board */
Wosser1sProductions 91:1dd4cdaede06 40 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
Wosser1sProductions 91:1dd4cdaede06 41
Wosser1sProductions 91:1dd4cdaede06 42 /* Retrieve the composing elements of the expansion board */
Wosser1sProductions 91:1dd4cdaede06 43 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
Wosser1sProductions 91:1dd4cdaede06 44 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
Wosser1sProductions 91:1dd4cdaede06 45 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
Wosser1sProductions 91:1dd4cdaede06 46 static HumiditySensor *humidity_sensor = mems_expansion_board->ht_sensor;
Wosser1sProductions 91:1dd4cdaede06 47 static PressureSensor *pressure_sensor = mems_expansion_board->pt_sensor;
Wosser1sProductions 91:1dd4cdaede06 48 static TempSensor *temp_sensor1 = mems_expansion_board->ht_sensor;
Wosser1sProductions 91:1dd4cdaede06 49 static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
Wosser1sProductions 91:1dd4cdaede06 50
Wosser1sProductions 91:1dd4cdaede06 51 DigitalOut status_led(LED1);
Wosser1sProductions 91:1dd4cdaede06 52
Wosser1sProductions 91:1dd4cdaede06 53 static SensorData MEMS_Data = { 0.0, 0.0,
Wosser1sProductions 91:1dd4cdaede06 54 0.0, 0.0,
Wosser1sProductions 91:1dd4cdaede06 55 0.0,
Wosser1sProductions 91:1dd4cdaede06 56 { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 },
Wosser1sProductions 91:1dd4cdaede06 57 { 0, 0, 0 },
Wosser1sProductions 91:1dd4cdaede06 58 false,
Wosser1sProductions 91:1dd4cdaede06 59 { 1, 0, 0}
Wosser1sProductions 91:1dd4cdaede06 60 };
Wosser1sProductions 91:1dd4cdaede06 61
Wosser1sProductions 91:1dd4cdaede06 62 static float rawFloat = 0.0;
Wosser1sProductions 91:1dd4cdaede06 63 static int32_t rawVector[3] = { 0, 0, 0};
Wosser1sProductions 91:1dd4cdaede06 64
Wosser1sProductions 91:1dd4cdaede06 65 /* Code ----------------------------------------------------------------------*/
Wosser1sProductions 91:1dd4cdaede06 66 // HTS221
Wosser1sProductions 91:1dd4cdaede06 67 #define getTemp_1(V) temp_sensor1->GetTemperature(&rawFloat); V = rawFloat
Wosser1sProductions 91:1dd4cdaede06 68 #define getTemp_1_Fahr(V) temp_sensor1->GetFahrenheit(&rawFloat); V = rawFloat
Wosser1sProductions 91:1dd4cdaede06 69 #define getHumidity(V) humidity_sensor->GetHumidity(&rawFloat); V = rawFloat
Wosser1sProductions 91:1dd4cdaede06 70
Wosser1sProductions 91:1dd4cdaede06 71 // LPS25H
Wosser1sProductions 91:1dd4cdaede06 72 #define getTemp_2(V) temp_sensor2->GetTemperature(&rawFloat); V = rawFloat
Wosser1sProductions 91:1dd4cdaede06 73 #define getTemp_2_Fahr(V) temp_sensor2->GetFahrenheit(&rawFloat); V = rawFloat
Wosser1sProductions 91:1dd4cdaede06 74 #define getPressure(V) pressure_sensor->GetPressure(&rawFloat); V = rawFloat
Wosser1sProductions 91:1dd4cdaede06 75
Wosser1sProductions 91:1dd4cdaede06 76 // LIS3MDL
Wosser1sProductions 91:1dd4cdaede06 77 #define getMagnetoVector(V) magnetometer->Get_M_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
Wosser1sProductions 91:1dd4cdaede06 78
Wosser1sProductions 91:1dd4cdaede06 79 // LSM6DS0
Wosser1sProductions 91:1dd4cdaede06 80 #define getAcceleroVector(V) accelerometer->Get_X_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
Wosser1sProductions 91:1dd4cdaede06 81 #define getGyroVector(V) gyroscope->Get_G_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
Wosser1sProductions 91:1dd4cdaede06 82
Wosser1sProductions 91:1dd4cdaede06 83 template <typename Ta, typename Tb, typename To>
Wosser1sProductions 91:1dd4cdaede06 84 void vector_cross(const vector<Ta>& a, const vector<Tb>& b, vector<To>& out){
Wosser1sProductions 91:1dd4cdaede06 85 out.x = (a.y * b.z) - (a.z * b.y);
Wosser1sProductions 91:1dd4cdaede06 86 out.y = (a.z * b.x) - (a.x * b.z);
Wosser1sProductions 91:1dd4cdaede06 87 out.z = (a.x * b.y) - (a.y * b.x);
Wosser1sProductions 91:1dd4cdaede06 88 }
Wosser1sProductions 91:1dd4cdaede06 89
Wosser1sProductions 91:1dd4cdaede06 90 template <typename Ta, typename Tb>
Wosser1sProductions 91:1dd4cdaede06 91 float vector_dot(const vector<Ta>& a, const vector<Tb>& b) {
Wosser1sProductions 91:1dd4cdaede06 92 return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
Wosser1sProductions 91:1dd4cdaede06 93 }
Wosser1sProductions 91:1dd4cdaede06 94
Wosser1sProductions 91:1dd4cdaede06 95 void vector_normalize(vector<float>& a) {
Wosser1sProductions 91:1dd4cdaede06 96 float mag = sqrt(vector_dot(a, a));
Wosser1sProductions 91:1dd4cdaede06 97 a.x /= mag;
Wosser1sProductions 91:1dd4cdaede06 98 a.y /= mag;
Wosser1sProductions 91:1dd4cdaede06 99 a.z /= mag;
Wosser1sProductions 91:1dd4cdaede06 100 }
Wosser1sProductions 91:1dd4cdaede06 101
Wosser1sProductions 91:1dd4cdaede06 102 void printMEMS_info(void) {
Wosser1sProductions 91:1dd4cdaede06 103 uint8_t id;
Wosser1sProductions 91:1dd4cdaede06 104
Wosser1sProductions 91:1dd4cdaede06 105 humidity_sensor->ReadID(&id);
Wosser1sProductions 91:1dd4cdaede06 106 printf("HTS221 humidity & temperature = 0x%X\r\n", id);
Wosser1sProductions 91:1dd4cdaede06 107 pressure_sensor->ReadID(&id);
Wosser1sProductions 91:1dd4cdaede06 108 printf("LPS25H pressure & temperature = 0x%X\r\n", id);
Wosser1sProductions 91:1dd4cdaede06 109 magnetometer->ReadID(&id);
Wosser1sProductions 91:1dd4cdaede06 110 printf("LIS3MDL magnetometer = 0x%X\r\n", id);
Wosser1sProductions 91:1dd4cdaede06 111 gyroscope->ReadID(&id);
Wosser1sProductions 91:1dd4cdaede06 112 printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);
Wosser1sProductions 91:1dd4cdaede06 113 }
Wosser1sProductions 91:1dd4cdaede06 114
Wosser1sProductions 91:1dd4cdaede06 115 void calibrateMagnetometer(void) {
Wosser1sProductions 91:1dd4cdaede06 116 if (!MEMS_Data.mag_isCalibrated) {
Wosser1sProductions 91:1dd4cdaede06 117 int16_t i;
Wosser1sProductions 91:1dd4cdaede06 118 vector<int32_t> rawvect,
Wosser1sProductions 91:1dd4cdaede06 119 mag_min = {+32767, +32767, +32767},
Wosser1sProductions 91:1dd4cdaede06 120 mag_max = {-32767, -32767, -32767};
Wosser1sProductions 91:1dd4cdaede06 121
Wosser1sProductions 91:1dd4cdaede06 122 printf("Calibrating Magnetometer: Move MEMS board in figure 8 while rotating in place for ~10 seconds...\r\n");
Wosser1sProductions 91:1dd4cdaede06 123
Wosser1sProductions 91:1dd4cdaede06 124 for (i = 0; i < 9750; ++i) {
Wosser1sProductions 91:1dd4cdaede06 125 getMagnetoVector(rawvect);
Wosser1sProductions 91:1dd4cdaede06 126
Wosser1sProductions 91:1dd4cdaede06 127 vector_minimize(mag_min, rawvect);
Wosser1sProductions 91:1dd4cdaede06 128 vector_maximize(mag_max, rawvect);
Wosser1sProductions 91:1dd4cdaede06 129
Wosser1sProductions 91:1dd4cdaede06 130 if (i % 50 == 0) status_led = !status_led.read();
Wosser1sProductions 91:1dd4cdaede06 131 wait_ms(1);
Wosser1sProductions 91:1dd4cdaede06 132 }
Wosser1sProductions 91:1dd4cdaede06 133
Wosser1sProductions 91:1dd4cdaede06 134 MEMS_Data.mag_avg = (vector<int32_t>) { ((mag_min.x + mag_max.x) / 2),
Wosser1sProductions 91:1dd4cdaede06 135 ((mag_min.y + mag_max.y) / 2),
Wosser1sProductions 91:1dd4cdaede06 136 ((mag_min.z + mag_max.z) / 2)
Wosser1sProductions 91:1dd4cdaede06 137 };
Wosser1sProductions 91:1dd4cdaede06 138 status_led = 1;
Wosser1sProductions 91:1dd4cdaede06 139 printf("Calibrating Magnetometer done.\r\n");
Wosser1sProductions 91:1dd4cdaede06 140 MEMS_Data.mag_isCalibrated = true;
Wosser1sProductions 91:1dd4cdaede06 141 } else
Wosser1sProductions 91:1dd4cdaede06 142 printf("Magnetometer is calibrated.\r\n");
Wosser1sProductions 91:1dd4cdaede06 143 }
Wosser1sProductions 91:1dd4cdaede06 144
Wosser1sProductions 91:1dd4cdaede06 145 /*
Wosser1sProductions 91:1dd4cdaede06 146 * Default heading: (vector<int>) {1, 0, 0}
Wosser1sProductions 91:1dd4cdaede06 147 *
Wosser1sProductions 91:1dd4cdaede06 148 * getMagnetoVector(V) and getAcceleroVector(V) should by called prior to this.
Wosser1sProductions 91:1dd4cdaede06 149 */
Wosser1sProductions 91:1dd4cdaede06 150 void getCompassheading(float& heading) {
Wosser1sProductions 91:1dd4cdaede06 151 // subtract offset (average of min and max) from magnetometer readings
Wosser1sProductions 91:1dd4cdaede06 152 vector<int> temp_m = { MEMS_Data.mag.x - MEMS_Data.mag_avg.x,
Wosser1sProductions 91:1dd4cdaede06 153 MEMS_Data.mag.y - MEMS_Data.mag_avg.y,
Wosser1sProductions 91:1dd4cdaede06 154 MEMS_Data.mag.z - MEMS_Data.mag_avg.z
Wosser1sProductions 91:1dd4cdaede06 155 };
Wosser1sProductions 91:1dd4cdaede06 156 // compute E and N
Wosser1sProductions 91:1dd4cdaede06 157 vector<float> E, N;
Wosser1sProductions 91:1dd4cdaede06 158 vector_cross(temp_m, MEMS_Data.acc, E);
Wosser1sProductions 91:1dd4cdaede06 159 vector_normalize(E);
Wosser1sProductions 91:1dd4cdaede06 160 vector_cross(MEMS_Data.acc, E, N);
Wosser1sProductions 91:1dd4cdaede06 161 vector_normalize(N);
Wosser1sProductions 91:1dd4cdaede06 162
Wosser1sProductions 91:1dd4cdaede06 163 // compute heading
Wosser1sProductions 91:1dd4cdaede06 164 heading = RAD2DEG(atan2((double)vector_dot(E, MEMS_Data.defaultHeading), (double)vector_dot(N, MEMS_Data.defaultHeading)));
Wosser1sProductions 91:1dd4cdaede06 165 if (heading < 0) heading += 360;
Wosser1sProductions 91:1dd4cdaede06 166 }
Wosser1sProductions 91:1dd4cdaede06 167
Wosser1sProductions 91:1dd4cdaede06 168 void updateSensorData(void) {
Wosser1sProductions 91:1dd4cdaede06 169 if (!MEMS_Data.mag_isCalibrated)
Wosser1sProductions 91:1dd4cdaede06 170 calibrateMagnetometer();
Wosser1sProductions 91:1dd4cdaede06 171
Wosser1sProductions 91:1dd4cdaede06 172 getTemp_1(MEMS_Data.temp_1);
Wosser1sProductions 91:1dd4cdaede06 173 getHumidity(MEMS_Data.humidity);
Wosser1sProductions 91:1dd4cdaede06 174
Wosser1sProductions 91:1dd4cdaede06 175 getTemp_2(MEMS_Data.temp_2);
Wosser1sProductions 91:1dd4cdaede06 176 getPressure(MEMS_Data.pressure);
Wosser1sProductions 91:1dd4cdaede06 177
Wosser1sProductions 91:1dd4cdaede06 178 getMagnetoVector(MEMS_Data.mag);
Wosser1sProductions 91:1dd4cdaede06 179 getAcceleroVector(MEMS_Data.acc);
Wosser1sProductions 91:1dd4cdaede06 180 getGyroVector(MEMS_Data.gyro);
Wosser1sProductions 91:1dd4cdaede06 181
Wosser1sProductions 91:1dd4cdaede06 182 getCompassheading(MEMS_Data.heading);
Wosser1sProductions 91:1dd4cdaede06 183 }
Wosser1sProductions 91:1dd4cdaede06 184
Wosser1sProductions 91:1dd4cdaede06 185 void printSensorData(void) {
Wosser1sProductions 91:1dd4cdaede06 186 printf("HTS221: [temp] % 3.2f%sC, [hum] % 3.2f%%\r\n", MEMS_Data.temp_1, DEGREES, MEMS_Data.humidity);
Wosser1sProductions 91:1dd4cdaede06 187 printf("LPS25H: [temp] % 3.2f%sC, [press] %4.2f mbar\r\n", MEMS_Data.temp_2, DEGREES, MEMS_Data.pressure);
Wosser1sProductions 91:1dd4cdaede06 188 printf("LIS3MDL [mag/mgauss]: %6ld, %6ld, %6ld\r\n", MEMS_Data.mag.x, MEMS_Data.mag.y, MEMS_Data.mag.z);
Wosser1sProductions 91:1dd4cdaede06 189 printf("LSM6DS0 [acc/mg]: %6ld, %6ld, %6ld\r\n", MEMS_Data.acc.x, MEMS_Data.acc.y, MEMS_Data.acc.z);
Wosser1sProductions 91:1dd4cdaede06 190 printf(" [Heading]: %3.2f%s\r\n", MEMS_Data.heading, DEGREES);
Wosser1sProductions 91:1dd4cdaede06 191 printf("LSM6DS0 [gyro/mdps]: %6ld, %6ld, %6ld\r\n", MEMS_Data.gyro.x, MEMS_Data.gyro.y, MEMS_Data.gyro.z);
Wosser1sProductions 91:1dd4cdaede06 192 }
Wosser1sProductions 91:1dd4cdaede06 193
Wosser1sProductions 91:1dd4cdaede06 194 #endif /* MEMS_SENSOR_WRAPPER_H */