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
- Committer:
- Wosser1sProductions
- Date:
- 2016-10-16
- Revision:
- 91:1dd4cdaede06
File content as of revision 91:1dd4cdaede06:
/* 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 */