sensors.cpp

00001 #include "mbed.h"
00002 #include "dbg.h"
00003 #include "sensors.h"
00004 #include "simul.h"
00005 
00006 #define CALL_METH(obj, meth, param, ret) ((obj == NULL) ?       \
00007                       ((*(param) = (ret)), 0) : \
00008                       ((obj)->meth(param))      \
00009                       )
00010 
00011 LIS3MDL *magnetometer;
00012 LSM6DS0 *accelerometer;
00013 LSM6DS0 *gyroscope;
00014 LPS25H *pressure_sensor;
00015 LPS25H *temp_sensor2;
00016 HTS221 *humidity_sensor;
00017 HTS221 *temp_sensor1;
00018 
00019 __inline int32_t float2_to_int(float v)
00020 {
00021     return (int32_t)(v*100);
00022 }
00023 
00024 bool Init_HTS221(HTS221* ht_sensor)
00025 {
00026     uint8_t ht_id = 0;
00027     HUM_TEMP_InitTypeDef InitStructure;
00028 
00029     /* Check presence */
00030     if((ht_sensor->ReadID(&ht_id) != HUM_TEMP_OK) ||
00031        (ht_id != I_AM_HTS221))
00032         {
00033             delete ht_sensor;
00034             ht_sensor = NULL;
00035             return true;
00036         }
00037     
00038     /* Configure sensor */
00039     InitStructure.OutputDataRate = HTS221_ODR_12_5Hz;
00040 
00041     if(ht_sensor->Init(&InitStructure) != HUM_TEMP_OK)
00042     {
00043         return false;
00044     }
00045 
00046     return true;
00047 }
00048 
00049 bool Init_LIS3MDL(LIS3MDL* magnetometer)
00050 {
00051     uint8_t m_id = 0;
00052     MAGNETO_InitTypeDef InitStructure;
00053 
00054     /* Check presence */
00055     if((magnetometer->ReadID(&m_id) != MAGNETO_OK) ||
00056        (m_id != I_AM_LIS3MDL_M))
00057     {
00058         delete magnetometer;
00059         magnetometer = NULL;
00060         return true;
00061     }
00062       
00063     /* Configure sensor */
00064     InitStructure.M_FullScale = LIS3MDL_M_FS_4;
00065     InitStructure.M_OperatingMode = LIS3MDL_M_MD_CONTINUOUS;
00066     InitStructure.M_XYOperativeMode = LIS3MDL_M_OM_HP;
00067     InitStructure.M_OutputDataRate = LIS3MDL_M_DO_80;
00068 
00069     if(magnetometer->Init(&InitStructure) != MAGNETO_OK)
00070     {
00071         return false;
00072     }
00073       
00074     return true;
00075 }
00076 
00077 bool Init_LPS25H(LPS25H* pt_sensor)
00078 {
00079     uint8_t p_id = 0;
00080     PRESSURE_InitTypeDef InitStructure;
00081     
00082     /* Check presence */
00083     if((pt_sensor->ReadID(&p_id) != PRESSURE_OK) ||
00084        (p_id != I_AM_LPS25H))
00085     {
00086         delete pt_sensor;
00087         pt_sensor = NULL;
00088         return true;
00089     }
00090             
00091     /* Configure sensor */
00092     InitStructure.OutputDataRate = LPS25H_ODR_1Hz;
00093     InitStructure.BlockDataUpdate = LPS25H_BDU_CONT;
00094     InitStructure.DiffEnable = LPS25H_DIFF_DISABLE;
00095     InitStructure.SPIMode = LPS25H_SPI_SIM_4W;
00096     InitStructure.PressureResolution = LPS25H_P_RES_AVG_8;
00097     InitStructure.TemperatureResolution = LPS25H_T_RES_AVG_8;
00098         
00099     if(pt_sensor->Init(&InitStructure) != PRESSURE_OK)
00100     {
00101         return false;
00102     }
00103     
00104     return true;
00105 }
00106 
00107 bool Init_LSM6DS0(LSM6DS0* gyro_lsm6ds0)
00108 {
00109     IMU_6AXES_InitTypeDef InitStructure;
00110     uint8_t xg_id = 0;
00111 
00112     /* Check presence */
00113     if((gyro_lsm6ds0->ReadID(&xg_id) != IMU_6AXES_OK) ||
00114        (xg_id != I_AM_LSM6DS0_XG))
00115     {
00116         delete gyro_lsm6ds0;
00117         gyro_lsm6ds0 = NULL;
00118         return true;
00119     }
00120             
00121     /* Configure sensor */
00122     InitStructure.G_FullScale       = 2000.0f; /* 2000DPS */
00123     InitStructure.G_OutputDataRate  = 119.0f;  /* 119HZ */
00124     InitStructure.G_X_Axis          = 1;       /* Enable */
00125     InitStructure.G_Y_Axis          = 1;       /* Enable */
00126     InitStructure.G_Z_Axis          = 1;       /* Enable */
00127 
00128     InitStructure.X_FullScale       = 2.0f;    /* 2G */
00129     InitStructure.X_OutputDataRate  = 119.0f;  /* 119HZ */
00130     InitStructure.X_X_Axis          = 1;       /* Enable */
00131     InitStructure.X_Y_Axis          = 1;       /* Enable */
00132     InitStructure.X_Z_Axis          = 1;       /* Enable */
00133               
00134     if(gyro_lsm6ds0->Init(&InitStructure) != IMU_6AXES_OK)
00135     {
00136         return false; 
00137     }
00138             
00139     return true;
00140 }
00141 
00142 bool mag_get_value(int32_t* buf)
00143 {
00144 #if _SENSORS_SIMU_
00145     return simul_sensor_value(buf, 3, -1900, 1900);
00146 #else
00147     return CALL_METH(magnetometer, Get_M_Axes, buf, 0)? true : false;
00148 #endif
00149 }
00150 
00151 bool acc_get_value(int32_t* buf)
00152 {
00153 #if _SENSORS_SIMU_
00154     return simul_sensor_value(buf, 3, -1900, 1900);
00155 #else
00156     return CALL_METH(accelerometer, Get_X_Axes, buf, 0)? true : false;
00157 #endif
00158 }
00159 
00160 bool gyr_get_value(int32_t* buf)
00161 {
00162 #if _SENSORS_SIMU_
00163     return simul_sensor_value(buf, 3, -40000, 40000);
00164 #else
00165     return CALL_METH(gyroscope, Get_G_Axes, buf, 0)? true : false;
00166 #endif
00167 }
00168 
00169 bool pre_get_value(int32_t* buf)
00170 {
00171 #if _SENSORS_SIMU_
00172     return simul_sensor_value(buf, 1, 96000, 104000);
00173 #else
00174     bool err;
00175     float tmp;
00176     err = CALL_METH(pressure_sensor, GetPressure, &tmp, 0.0f)? true : false;
00177     buf[0] = float2_to_int(tmp);
00178     return err;
00179 #endif
00180 }
00181 
00182 bool hum_get_value(int32_t* buf)
00183 {
00184 #if _SENSORS_SIMU_
00185     return simul_sensor_value(buf, 1, 1000, 9000);
00186 #else
00187     bool err;
00188     float tmp;
00189     err = CALL_METH(humidity_sensor, GetHumidity, &tmp, 0.0f)? true : false;
00190     buf[0] = float2_to_int(tmp);
00191     return err;
00192 #endif
00193 }
00194 
00195 bool tem1_get_value(int32_t* buf)
00196 {
00197 #if _SENSORS_SIMU_
00198     return simul_sensor_value(buf, 1, 1100, 3900);
00199 #else
00200     bool err;
00201     float tmp;
00202     err = CALL_METH(temp_sensor1, GetTemperature, &tmp, 0.0f)? true : false;
00203     buf[0] = float2_to_int(tmp);
00204     return err;
00205 #endif
00206 }
00207 
00208 bool tem2_get_value(int32_t* buf)
00209 {
00210 #if _SENSORS_SIMU_
00211     return simul_sensor_value(buf, 1, 5100, 10100);
00212 #else
00213     bool err;
00214     float tmp;
00215     err = CALL_METH(temp_sensor2, GetFahrenheit, &tmp, 0.0f)? true : false;
00216     buf[0] = float2_to_int(tmp);
00217     return err;
00218 #endif
00219 }