MEMS_Sensor_wrapper.hpp

00001 /* Define to prevent from recursive inclusion --------------------------------*/
00002 #ifndef MEMS_SENSOR_WRAPPER_H
00003 #define MEMS_SENSOR_WRAPPER_H
00004 
00005 /* Includes ------------------------------------------------------------------*/
00006 #include "mbed.h"
00007 #include "x_nucleo_iks01a1.h"
00008 
00009 /* Macros/typedefs -----------------------------------------------------------*/
00010 #define DEGREES                 "\u00B0"
00011 #define PI                      3.14159265
00012 
00013 #define RAD2DEG(X)              ((X * 180) / PI)
00014 
00015 #define DATA2VECTOR(D, T)       (vector<T>) { D[0], D[1], D[2] }
00016 
00017 #define min(X, Y)               (X < Y ? X : Y)
00018 #define max(X, Y)               (X > Y ? X : Y)
00019 
00020 #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)
00021 #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)
00022 
00023 template <typename T> 
00024 struct vector {
00025   T x, y, z;
00026 };
00027 
00028 typedef struct {
00029     float temp_1, temp_2;
00030     float humidity, pressure;
00031     float heading;
00032     vector<int32_t> mag, acc, gyro;
00033     vector<int32_t> mag_avg;
00034     bool mag_isCalibrated;
00035     vector<int16_t> defaultHeading;
00036 } SensorData;
00037 
00038 /* Variable definitions ------------------------------------------------------*/
00039 /* Instantiate the expansion board */
00040 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
00041 
00042 /* Retrieve the composing elements of the expansion board */
00043 static GyroSensor       *gyroscope          = mems_expansion_board->GetGyroscope();
00044 static MotionSensor     *accelerometer      = mems_expansion_board->GetAccelerometer();
00045 static MagneticSensor   *magnetometer       = mems_expansion_board->magnetometer;
00046 static HumiditySensor   *humidity_sensor    = mems_expansion_board->ht_sensor;
00047 static PressureSensor   *pressure_sensor    = mems_expansion_board->pt_sensor;
00048 static TempSensor       *temp_sensor1       = mems_expansion_board->ht_sensor;
00049 static TempSensor       *temp_sensor2       = mems_expansion_board->pt_sensor;
00050 
00051 DigitalOut              status_led(LED1);
00052 
00053 static SensorData       MEMS_Data = { 0.0, 0.0,
00054                                       0.0, 0.0,
00055                                       0.0,
00056                                       { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 },
00057                                       { 0, 0, 0 },
00058                                       false,
00059                                       { 1, 0, 0}
00060                                     };
00061 
00062 static float            rawFloat            = 0.0;
00063 static int32_t          rawVector[3]        = { 0, 0, 0};
00064 
00065 /* Code ----------------------------------------------------------------------*/
00066 // HTS221
00067 #define getTemp_1(V)            temp_sensor1->GetTemperature(&rawFloat); V = rawFloat
00068 #define getTemp_1_Fahr(V)       temp_sensor1->GetFahrenheit(&rawFloat); V = rawFloat
00069 #define getHumidity(V)          humidity_sensor->GetHumidity(&rawFloat); V = rawFloat
00070 
00071 // LPS25H
00072 #define getTemp_2(V)            temp_sensor2->GetTemperature(&rawFloat); V = rawFloat
00073 #define getTemp_2_Fahr(V)       temp_sensor2->GetFahrenheit(&rawFloat); V = rawFloat
00074 #define getPressure(V)          pressure_sensor->GetPressure(&rawFloat); V = rawFloat
00075 
00076 // LIS3MDL
00077 #define getMagnetoVector(V)     magnetometer->Get_M_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
00078 
00079 // LSM6DS0
00080 #define getAcceleroVector(V)    accelerometer->Get_X_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
00081 #define getGyroVector(V)        gyroscope->Get_G_Axes(rawVector); V = DATA2VECTOR(rawVector, int32_t)
00082 
00083 template <typename Ta, typename Tb, typename To> 
00084 void vector_cross(const vector<Ta>& a, const vector<Tb>& b, vector<To>& out){
00085     out.x = (a.y * b.z) - (a.z * b.y);
00086     out.y = (a.z * b.x) - (a.x * b.z);
00087     out.z = (a.x * b.y) - (a.y * b.x);
00088 }
00089 
00090 template <typename Ta, typename Tb> 
00091 float vector_dot(const vector<Ta>& a, const vector<Tb>& b) {
00092     return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
00093 }
00094 
00095 void vector_normalize(vector<float>& a) {
00096     float mag = sqrt(vector_dot(a, a));
00097     a.x /= mag;
00098     a.y /= mag;
00099     a.z /= mag;
00100 }
00101 
00102 void printMEMS_info(void) {
00103     uint8_t id;
00104     
00105     humidity_sensor->ReadID(&id);
00106     printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
00107     pressure_sensor->ReadID(&id);
00108     printf("LPS25H  pressure & temperature    = 0x%X\r\n", id);
00109     magnetometer->ReadID(&id);
00110     printf("LIS3MDL magnetometer              = 0x%X\r\n", id);
00111     gyroscope->ReadID(&id);
00112     printf("LSM6DS0 accelerometer & gyroscope = 0x%X\r\n", id);   
00113 }
00114 
00115 void calibrateMagnetometer(void) {
00116     if (!MEMS_Data.mag_isCalibrated) {
00117         int16_t i;
00118         vector<int32_t> rawvect,
00119                         mag_min = {+32767, +32767, +32767},
00120                         mag_max = {-32767, -32767, -32767};
00121         
00122         printf("Calibrating Magnetometer: Move MEMS board in figure 8 while rotating in place for ~10 seconds...\r\n");
00123         
00124         for (i = 0; i < 9750; ++i) {
00125             getMagnetoVector(rawvect);
00126             
00127             vector_minimize(mag_min, rawvect);
00128             vector_maximize(mag_max, rawvect);
00129             
00130             if (i % 50 == 0) status_led = !status_led.read();
00131             wait_ms(1);
00132         }
00133         
00134         MEMS_Data.mag_avg = (vector<int32_t>) { ((mag_min.x + mag_max.x) / 2),
00135                                                 ((mag_min.y + mag_max.y) / 2),
00136                                                 ((mag_min.z + mag_max.z) / 2)
00137                                               };
00138         status_led = 1;
00139         printf("Calibrating Magnetometer done.\r\n");
00140         MEMS_Data.mag_isCalibrated = true;
00141     } else
00142         printf("Magnetometer is calibrated.\r\n");
00143 }
00144 
00145 /*
00146  *  Default heading: (vector<int>) {1, 0, 0}
00147  *  
00148  *  getMagnetoVector(V) and getAcceleroVector(V) should by called prior to this.
00149  */
00150 void getCompassheading(float& heading) {
00151     // subtract offset (average of min and max) from magnetometer readings
00152     vector<int> temp_m = {  MEMS_Data.mag.x - MEMS_Data.mag_avg.x, 
00153                             MEMS_Data.mag.y - MEMS_Data.mag_avg.y, 
00154                             MEMS_Data.mag.z - MEMS_Data.mag_avg.z
00155                          };
00156     // compute E and N
00157     vector<float> E, N;
00158     vector_cross(temp_m, MEMS_Data.acc, E);
00159     vector_normalize(E);
00160     vector_cross(MEMS_Data.acc, E, N);
00161     vector_normalize(N);
00162 
00163     // compute heading
00164     heading = RAD2DEG(atan2((double)vector_dot(E, MEMS_Data.defaultHeading), (double)vector_dot(N, MEMS_Data.defaultHeading)));
00165     if (heading < 0) heading += 360;
00166 }
00167 
00168 void updateSensorData(void) {  
00169     if (!MEMS_Data.mag_isCalibrated)
00170         calibrateMagnetometer();
00171         
00172     getTemp_1(MEMS_Data.temp_1);
00173     getHumidity(MEMS_Data.humidity);
00174     
00175     getTemp_2(MEMS_Data.temp_2);
00176     getPressure(MEMS_Data.pressure);
00177 
00178     getMagnetoVector(MEMS_Data.mag);
00179     getAcceleroVector(MEMS_Data.acc);
00180     getGyroVector(MEMS_Data.gyro);
00181     
00182     getCompassheading(MEMS_Data.heading);
00183 }
00184 
00185 void printSensorData(void) {
00186     printf("HTS221: [temp] % 3.2f%sC,   [hum]  % 3.2f%%\r\n", MEMS_Data.temp_1, DEGREES, MEMS_Data.humidity);
00187     printf("LPS25H: [temp] % 3.2f%sC, [press] %4.2f mbar\r\n", MEMS_Data.temp_2, DEGREES, MEMS_Data.pressure);
00188     printf("LIS3MDL [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", MEMS_Data.mag.x, MEMS_Data.mag.y, MEMS_Data.mag.z);
00189     printf("LSM6DS0 [acc/mg]:      %6ld, %6ld, %6ld\r\n", MEMS_Data.acc.x, MEMS_Data.acc.y, MEMS_Data.acc.z);    
00190     printf("        [Heading]:     %3.2f%s\r\n", MEMS_Data.heading, DEGREES);
00191     printf("LSM6DS0 [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", MEMS_Data.gyro.x, MEMS_Data.gyro.y, MEMS_Data.gyro.z);
00192 }
00193 
00194 #endif /* MEMS_SENSOR_WRAPPER_H */