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
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?
User | Revision | Line number | New 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 */ |