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 */