William Thenaers
/
X_NUCLEO_IKS01A1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: ST_INTERFACES X_NUCLEO_COMMON
Dependents: HelloWorld_IKS01A1
Fork of
X_NUCLEO_IKS01A1
by 
ST
Revision 91:1dd4cdaede06, committed 2016-10-16
- Comitter:
- Wosser1sProductions
- Date:
- Sun Oct 16 05:40:03 2016 +0000
- Parent:
- 90:bd74c33ecbbd
- Commit message:
- Commit
Changed in this revision
| MEMS_Sensor_wrapper.hpp | Show annotated file Show diff for this revision Revisions of this file |
--- /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 */