File content as of revision 33:115aa467b4fa:

#include "mbed.h"
#include "x_nucleo_iks01a1.h"
#include "mailBox.cpp"

using namespace std;


/* Instantiate the expansion board */
static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);

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;

static Ticker ticker;
void readData();

class ExpansionBoard {
public:
    float T;
    int count;
    
private:
    /* Helper function for printing floats & doubles */
    static char *printDouble(char* str, double v, int decimalDigits=2) {
        int i = 1;
        int intPart, fractPart;
        int len;
        char *ptr;
        /* prepare decimal digits multiplicator */
        for (; decimalDigits!=0; i*=10, decimalDigits--);
        /* calculate integer & fractinal parts */
        intPart = (int)v;
        fractPart = (int)((v-(double)(int)v)*i);
        /* fill in integer part */
        sprintf(str, "%i.", intPart);
        /* prepare fill in of fractional part */
        len = strlen(str);
        ptr = &str[len];
        /* fill in leading fractional zeros */
        for (i/=10; i>1; i/=10, ptr++) {
            if(fractPart >= i) break;
            *ptr = '0'; }
        /* fill in (rest of) fractional part */
        sprintf(ptr, "%i", fractPart);
        return str;
    }
    
    //This is the producer
    void readData() {
        float value1, value2;
        char buffer1[32], buffer2[32];
        int32_t axes[3];
        
        log_data *log_d = (log_data*)mail_box.alloc();
        //TODO Too small!?
        log_d->id = rand() % 255;
        
        //TODO Out of memory, specs say to delete oldest sample
        if (log_d == NULL) return;
        
        temp_sensor1->GetTemperature(&value1);
        humidity_sensor->GetHumidity(&value2);
        printf("HTS221: [temp] %7s°C,   [hum] %s%%\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
        log_d->tempCelsius = value1;
        log_d->humidity = value2;
        
        temp_sensor2->GetFahrenheit(&value1);
        pressure_sensor->GetPressure(&value2);
        printf("LPS25H: [temp] %7s°F, [press] %smbar\r\n", printDouble(buffer1, value1), printDouble(buffer2, value2));
        log_d->tempFarenheit = value1;
        log_d->pressure = value2;
        printf("---\r\n");

        magnetometer->Get_M_Axes(axes);
        printf("LIS3MDL [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
        memcpy(&log_d->magnetometer, &axes, sizeof(axes));
        
        accelerometer->Get_X_Axes(axes);
        printf("LSM6DS0 [acc/mg]:      %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
        memcpy(&log_d->accelerometer, &axes, sizeof(axes));
        
        gyroscope->Get_G_Axes(axes);
        printf("LSM6DS0 [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
        memcpy (&log_d->accelerometer, &axes, sizeof(axes));
        
        //TODO Not sure about this one, maybe use time()
        //log_d->date = asctime(localtime());
        
        //Send pointer to the queue
        osStatus *stat = (osStatus*)mail_box.put(log_d);
        count++;
        
        // Check for resource error
        if (stat == osErrorResource.osStatus) {
            printf("mail_box->put() Error %4Xh", stat);
            //Error, free up memory block
            mail_box.free(log_d);
            count--;
            return;
        }
    }
    
public:
    ExpansionBoard() : count(0) {
        /* Retrieve the composing elements of the expansion board */
        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 startSampling() {
        //TODO For now it just spits out the data
        //but data must be passed to the FIFO
        ticker.attach(this, &ExpansionBoard::readData, T);
    }
    void stopSampling() { ticker.detach(); }
};