File content as of revision 0:429446fe396d:

#include "mbed.h"
#include "dbg.h"
#include "sensors.h"
#include "simul.h"

#define CALL_METH(obj, meth, param, ret) ((obj == NULL) ?       \
                      ((*(param) = (ret)), 0) : \
                      ((obj)->meth(param))      \
                      )

LIS3MDL *magnetometer;
LSM6DS0 *accelerometer;
LSM6DS0 *gyroscope;
LPS25H *pressure_sensor;
LPS25H *temp_sensor2;
HTS221 *humidity_sensor;
HTS221 *temp_sensor1;

__inline int32_t float2_to_int(float v)
{
    return (int32_t)(v*100);
}

bool Init_HTS221(HTS221* ht_sensor)
{
    uint8_t ht_id = 0;
    HUM_TEMP_InitTypeDef InitStructure;

    /* Check presence */
    if((ht_sensor->ReadID(&ht_id) != HUM_TEMP_OK) ||
       (ht_id != I_AM_HTS221))
        {
            delete ht_sensor;
            ht_sensor = NULL;
            return true;
        }
    
    /* Configure sensor */
    InitStructure.OutputDataRate = HTS221_ODR_12_5Hz;

    if(ht_sensor->Init(&InitStructure) != HUM_TEMP_OK)
    {
        return false;
    }

    return true;
}

bool Init_LIS3MDL(LIS3MDL* magnetometer)
{
    uint8_t m_id = 0;
    MAGNETO_InitTypeDef InitStructure;

    /* Check presence */
    if((magnetometer->ReadID(&m_id) != MAGNETO_OK) ||
       (m_id != I_AM_LIS3MDL_M))
    {
        delete magnetometer;
        magnetometer = NULL;
        return true;
    }
      
    /* Configure sensor */
    InitStructure.M_FullScale = LIS3MDL_M_FS_4;
    InitStructure.M_OperatingMode = LIS3MDL_M_MD_CONTINUOUS;
    InitStructure.M_XYOperativeMode = LIS3MDL_M_OM_HP;
    InitStructure.M_OutputDataRate = LIS3MDL_M_DO_80;

    if(magnetometer->Init(&InitStructure) != MAGNETO_OK)
    {
        return false;
    }
      
    return true;
}

bool Init_LPS25H(LPS25H* pt_sensor)
{
    uint8_t p_id = 0;
    PRESSURE_InitTypeDef InitStructure;
    
    /* Check presence */
    if((pt_sensor->ReadID(&p_id) != PRESSURE_OK) ||
       (p_id != I_AM_LPS25H))
    {
        delete pt_sensor;
        pt_sensor = NULL;
        return true;
    }
            
    /* Configure sensor */
    InitStructure.OutputDataRate = LPS25H_ODR_1Hz;
    InitStructure.BlockDataUpdate = LPS25H_BDU_CONT;
    InitStructure.DiffEnable = LPS25H_DIFF_DISABLE;
    InitStructure.SPIMode = LPS25H_SPI_SIM_4W;
    InitStructure.PressureResolution = LPS25H_P_RES_AVG_8;
    InitStructure.TemperatureResolution = LPS25H_T_RES_AVG_8;
        
    if(pt_sensor->Init(&InitStructure) != PRESSURE_OK)
    {
        return false;
    }
    
    return true;
}

bool Init_LSM6DS0(LSM6DS0* gyro_lsm6ds0)
{
    IMU_6AXES_InitTypeDef InitStructure;
    uint8_t xg_id = 0;

    /* Check presence */
    if((gyro_lsm6ds0->ReadID(&xg_id) != IMU_6AXES_OK) ||
       (xg_id != I_AM_LSM6DS0_XG))
    {
        delete gyro_lsm6ds0;
        gyro_lsm6ds0 = NULL;
        return true;
    }
            
    /* Configure sensor */
    InitStructure.G_FullScale       = 2000.0f; /* 2000DPS */
    InitStructure.G_OutputDataRate  = 119.0f;  /* 119HZ */
    InitStructure.G_X_Axis          = 1;       /* Enable */
    InitStructure.G_Y_Axis          = 1;       /* Enable */
    InitStructure.G_Z_Axis          = 1;       /* Enable */

    InitStructure.X_FullScale       = 2.0f;    /* 2G */
    InitStructure.X_OutputDataRate  = 119.0f;  /* 119HZ */
    InitStructure.X_X_Axis          = 1;       /* Enable */
    InitStructure.X_Y_Axis          = 1;       /* Enable */
    InitStructure.X_Z_Axis          = 1;       /* Enable */
              
    if(gyro_lsm6ds0->Init(&InitStructure) != IMU_6AXES_OK)
    {
        return false; 
    }
            
    return true;
}

bool mag_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 3, -1900, 1900);
#else
    return CALL_METH(magnetometer, Get_M_Axes, buf, 0)? true : false;
#endif
}

bool acc_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 3, -1900, 1900);
#else
    return CALL_METH(accelerometer, Get_X_Axes, buf, 0)? true : false;
#endif
}

bool gyr_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 3, -40000, 40000);
#else
    return CALL_METH(gyroscope, Get_G_Axes, buf, 0)? true : false;
#endif
}

bool pre_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 1, 96000, 104000);
#else
    bool err;
    float tmp;
    err = CALL_METH(pressure_sensor, GetPressure, &tmp, 0.0f)? true : false;
    buf[0] = float2_to_int(tmp);
    return err;
#endif
}

bool hum_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 1, 1000, 9000);
#else
    bool err;
    float tmp;
    err = CALL_METH(humidity_sensor, GetHumidity, &tmp, 0.0f)? true : false;
    buf[0] = float2_to_int(tmp);
    return err;
#endif
}

bool tem1_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 1, 1100, 3900);
#else
    bool err;
    float tmp;
    err = CALL_METH(temp_sensor1, GetTemperature, &tmp, 0.0f)? true : false;
    buf[0] = float2_to_int(tmp);
    return err;
#endif
}

bool tem2_get_value(int32_t* buf)
{
#if _SENSORS_SIMU_
    return simul_sensor_value(buf, 1, 5100, 10100);
#else
    bool err;
    float tmp;
    err = CALL_METH(temp_sensor2, GetFahrenheit, &tmp, 0.0f)? true : false;
    buf[0] = float2_to_int(tmp);
    return err;
#endif
}