sensors.cpp

00001 #include "mbed.h"
00002 #include "WizziDebug.h"
00003 #include "sensors.h"
00004 #include "hwcfg.h"
00005 #include "simul.h"
00006 #include "sensors_cfg.h"
00007 
00008 #if defined(TARGET_STM32L152RE)
00009 LIS3MDL *magnetometer;
00010 LSM6DS0 *accelerometer;
00011 LSM6DS0 *gyroscope;
00012 #elif defined(TARGET_STM32L432KC)
00013 LSM303C_ACC_Sensor *accelerometer;
00014 LSM303C_MAG_Sensor *magnetometer;
00015     #if defined(SENSOR_LIGHT_MEAS) && defined(SENSOR_LIGHT_EN)
00016     AnalogIn g_light_meas(SENSOR_LIGHT_MEAS);
00017     DigitalOut g_light_en_l(SENSOR_LIGHT_EN);
00018     #endif
00019 #endif
00020 LPS25H *pressure_sensor;
00021 LPS25H *temp_sensor2;
00022 HTS221 *humidity_sensor;
00023 HTS221 *temp_sensor1;
00024 
00025 
00026 
00027 bool Init_HTS221(HTS221* ht_sensor)
00028 {
00029     uint8_t ht_id = 0;
00030     HUM_TEMP_InitTypeDef InitStructure;
00031 
00032     /* Check presence */
00033     if((ht_sensor->ReadID(&ht_id) != HUM_TEMP_OK) ||
00034        (ht_id != I_AM_HTS221))
00035         {
00036             delete ht_sensor;
00037             ht_sensor = NULL;
00038             return false;
00039         }
00040     
00041     /* Configure sensor */
00042     InitStructure.OutputDataRate = HTS221_ODR_12_5Hz;
00043 
00044     if(ht_sensor->Init(&InitStructure) != HUM_TEMP_OK)
00045     {
00046         delete ht_sensor;
00047         ht_sensor = NULL;
00048         return false;
00049     }
00050     
00051     return true;
00052 }
00053 
00054 bool Init_LIS3MDL(LIS3MDL* magnetometer)
00055 {
00056     uint8_t m_id = 0;
00057     MAGNETO_InitTypeDef InitStructure;
00058 
00059     /* Check presence */
00060     if((magnetometer->ReadID(&m_id) != MAGNETO_OK) ||
00061        (m_id != I_AM_LIS3MDL_M))
00062     {
00063         delete magnetometer;
00064         magnetometer = NULL;
00065         return false;
00066     }
00067       
00068     /* Configure sensor */
00069     InitStructure.M_FullScale = LIS3MDL_M_FS_4;
00070     InitStructure.M_OperatingMode = LIS3MDL_M_MD_CONTINUOUS;
00071     InitStructure.M_XYOperativeMode = LIS3MDL_M_OM_HP;
00072     InitStructure.M_OutputDataRate = LIS3MDL_M_DO_80;
00073 
00074     if(magnetometer->Init(&InitStructure) != MAGNETO_OK)
00075     {
00076         return false;
00077     }
00078       
00079     return true;
00080 }
00081 
00082 bool Init_LPS25H(LPS25H* pt_sensor)
00083 {
00084     uint8_t p_id = 0;
00085     PRESSURE_InitTypeDef InitStructure;
00086     
00087     /* Check presence */
00088     if((pt_sensor->ReadID(&p_id) != PRESSURE_OK) ||
00089        (p_id != I_AM_LPS25H))
00090     {
00091         delete pt_sensor;
00092         pt_sensor = NULL;
00093         return false;
00094     }
00095             
00096     /* Configure sensor */
00097     InitStructure.OutputDataRate = LPS25H_ODR_1Hz;
00098     InitStructure.BlockDataUpdate = LPS25H_BDU_CONT;
00099     InitStructure.DiffEnable = LPS25H_DIFF_DISABLE;
00100     InitStructure.SPIMode = LPS25H_SPI_SIM_4W;
00101     InitStructure.PressureResolution = LPS25H_P_RES_AVG_8;
00102     InitStructure.TemperatureResolution = LPS25H_T_RES_AVG_8;
00103         
00104     if(pt_sensor->Init(&InitStructure) != PRESSURE_OK)
00105     {
00106         return false;
00107     }
00108     
00109     return true;
00110 }
00111 
00112 bool Init_LSM6DS0(LSM6DS0* gyro_lsm6ds0)
00113 {
00114     IMU_6AXES_InitTypeDef InitStructure;
00115     uint8_t xg_id = 0;
00116 
00117     /* Check presence */
00118     if((gyro_lsm6ds0->ReadID(&xg_id) != IMU_6AXES_OK) ||
00119        (xg_id != I_AM_LSM6DS0_XG))
00120     {
00121         delete gyro_lsm6ds0;
00122         gyro_lsm6ds0 = NULL;
00123         return false;
00124     }
00125             
00126     /* Configure sensor */
00127     InitStructure.G_FullScale       = 2000.0f; /* 2000DPS */
00128     InitStructure.G_OutputDataRate  = 119.0f;  /* 119HZ */
00129     InitStructure.G_X_Axis          = 1;       /* Enable */
00130     InitStructure.G_Y_Axis          = 1;       /* Enable */
00131     InitStructure.G_Z_Axis          = 1;       /* Enable */
00132 
00133     InitStructure.X_FullScale       = 2.0f;    /* 2G */
00134     InitStructure.X_OutputDataRate  = 119.0f;  /* 119HZ */
00135     InitStructure.X_X_Axis          = 1;       /* Enable */
00136     InitStructure.X_Y_Axis          = 1;       /* Enable */
00137     InitStructure.X_Z_Axis          = 1;       /* Enable */
00138               
00139     if(gyro_lsm6ds0->Init(&InitStructure) != IMU_6AXES_OK)
00140     {
00141         return false;
00142     }
00143             
00144     return true;
00145 }
00146 
00147 bool Init_LSM303C_MAG(LSM303C_MAG_Sensor* magnetometer)
00148 {
00149     uint8_t id = 0;
00150     uint8_t error = 0;
00151     
00152     error = magnetometer->ReadID(&id);
00153     
00154     /* Check presence */
00155     if(error)
00156     {
00157         PRINT("LSM303C_MAG Not detected!\r\n");
00158         delete magnetometer;
00159         magnetometer = NULL;
00160         return false;
00161     }
00162     
00163     if (id != I_AM_LSM303C_MAG)
00164     {
00165         PRINT("This is not a LSM303C_MAG (0x02X != 0x02X)\r\n", id, I_AM_LSM303C_MAG);
00166         delete magnetometer;
00167         magnetometer = NULL;
00168         return false;
00169     }
00170     
00171     if(magnetometer->Init(NULL))
00172     {
00173         return false;
00174     }
00175     
00176     if(magnetometer->Set_M_ODR(1.250))
00177     {
00178         return false;
00179     }
00180     
00181     if(magnetometer->Enable())
00182     {
00183         return false;
00184     }
00185     
00186     return true;
00187 }
00188 
00189 bool Init_LSM303C_ACC(LSM303C_ACC_Sensor* accelerometer)
00190 {
00191     uint8_t id = 0;
00192     uint8_t error = 0;
00193     
00194     error = accelerometer->ReadID(&id);
00195     
00196     /* Check presence */
00197     if(error)
00198     {
00199         PRINT("LSM303C_ACC Not detected!\r\n");
00200         delete accelerometer;
00201         accelerometer = NULL;
00202         return false;
00203     }
00204     
00205     if (id != I_AM_LSM303C_ACC)
00206     {
00207         PRINT("This is not a LSM303C_ACC (0x02X != 0x02X)\r\n", id, I_AM_LSM303C_ACC);
00208         delete accelerometer;
00209         accelerometer = NULL;
00210         return false;
00211     }
00212     
00213     if(accelerometer->Init(NULL))
00214     {
00215         return false;
00216     }
00217     
00218     if(accelerometer->Set_X_ODR(10))
00219     {
00220         return false;
00221     }
00222     
00223     if(accelerometer->Enable())
00224     {
00225         return false;
00226     }
00227     
00228     return true;
00229 }
00230 
00231 // Cal method
00232 #define CALL_METH(obj, meth, param, ret) ((obj == NULL) ?       \
00233                       ((*(param) = (ret)), 0) : \
00234                       ((obj)->meth(param))      \
00235                       )
00236               
00237 __inline int32_t float2_to_int(float v)
00238 {
00239     return (int32_t)(v*100);
00240 }
00241 
00242 bool mag_get_value(int32_t* buf)
00243 {
00244 #if (_MAG_EN_ == 0)
00245     return simul_sensor_value(buf, 3, -1900, 1900);
00246 #elif (_MAG_EN_ == 1)
00247     return CALL_METH(magnetometer, Get_M_Axes, buf, 0)? true : false;
00248 #else 
00249     return false;
00250 #endif
00251 }
00252 
00253 bool acc_get_value(int32_t* buf)
00254 {
00255 #if (_ACC_EN_ == 0)
00256     return simul_sensor_value(buf, 3, -1900, 1900);
00257 #elif (_ACC_EN_ == 1)
00258     return CALL_METH(accelerometer, Get_X_Axes, buf, 0)? true : false;
00259 #else 
00260     return false;
00261 #endif
00262 }
00263 
00264 bool gyr_get_value(int32_t* buf)
00265 {
00266 #if (_GYR_EN_ == 0)
00267     return simul_sensor_value(buf, 3, -40000, 40000);
00268 #elif (_GYR_EN_ == 1)
00269     return CALL_METH(gyroscope, Get_G_Axes, buf, 0)? true : false;
00270 #else 
00271     return false;
00272 #endif
00273 }
00274 
00275 bool pre_get_value(int32_t* buf)
00276 {
00277 #if (_PRE_EN_ == 0)
00278     return simul_sensor_value(buf, 1, 96000, 104000);
00279 #elif (_PRE_EN_ == 1)
00280     bool err;
00281     float tmp;
00282     err = CALL_METH(pressure_sensor, GetPressure, &tmp, 0.0f)? true : false;
00283     buf[0] = float2_to_int(tmp);
00284     return err;
00285 #else 
00286     return false;
00287 #endif
00288 }
00289 
00290 bool hum_get_value(int32_t* buf)
00291 {
00292 #if (_HUM_EN_ == 0)
00293     return simul_sensor_value(buf, 1, 1000, 9000);
00294 #elif (_HUM_EN_ == 1)
00295     bool err;
00296     float tmp;
00297     err = CALL_METH(humidity_sensor, GetHumidity, &tmp, 0.0f)? true : false;
00298     buf[0] = float2_to_int(tmp);
00299     return err;
00300 #else
00301     return false;
00302 #endif
00303 }
00304 
00305 bool tem1_get_value(int32_t* buf)
00306 {
00307 #if (_TEM1_EN_ == 0)
00308     return simul_sensor_value(buf, 1, 1100, 3900);
00309 #elif (_TEM1_EN_ == 1)
00310     bool err;
00311     float tmp;
00312     err = CALL_METH(temp_sensor1, GetTemperature, &tmp, 0.0f)? true : false;
00313     buf[0] = float2_to_int(tmp);    
00314     return err;
00315 #else
00316     return false;
00317 #endif
00318 }
00319 
00320 bool tem2_get_value(int32_t* buf)
00321 {
00322 #if (_TEM2_EN_ == 0)
00323     return simul_sensor_value(buf, 1, 5100, 10100);
00324 #elif (_TEM2_EN_ == 1)
00325     bool err;
00326     float tmp;
00327     err = CALL_METH(temp_sensor2, GetFahrenheit, &tmp, 0.0f)? true : false;
00328     buf[0] = float2_to_int(tmp);
00329     return err;
00330 #else
00331     return false;
00332 #endif
00333 }
00334 
00335 bool light_get_value(int32_t* buf)
00336 {
00337 #if (_LIGHT_EN_ == 0)
00338     return simul_sensor_value(buf, 1, 0, 1000);
00339 #elif (_LIGHT_EN_ == 1) && defined(SENSOR_LIGHT_MEAS) && defined(SENSOR_LIGHT_EN)
00340     float tmp;
00341     g_light_en_l = 0;
00342     ThisThread::sleep_for(10);
00343     tmp = g_light_meas;
00344     g_light_en_l = 1;
00345     buf[0] = (int32_t)(tmp*1000);
00346     return false;
00347 #else
00348     return false;
00349 #endif
00350 }