Diff: sensors.cpp

Revision:

0:87c57e1b1e1c

Child:

1:4d3968b2941b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors.cpp	Mon May 15 16:00:25 2017 +0000
@@ -0,0 +1,348 @@
+#include "mbed.h"
+#include "WizziDebug.h"
+#include "sensors.h"
+#include "hwcfg.h"
+
+#if defined(TARGET_STM32L152RE)
+LIS3MDL *magnetometer;
+LSM6DS0 *accelerometer;
+LSM6DS0 *gyroscope;
+#elif defined(TARGET_STM32L432KC)
+LSM303C_ACC_Sensor *accelerometer;
+LSM303C_MAG_Sensor *magnetometer;
+    #if defined(SENSOR_LIGHT_MEAS) && defined(SENSOR_LIGHT_EN)
+    AnalogIn g_light_meas(SENSOR_LIGHT_MEAS);
+    DigitalOut g_light_en_l(SENSOR_LIGHT_EN);
+    #endif
+#endif
+LPS25H *pressure_sensor;
+LPS25H *temp_sensor2;
+HTS221 *humidity_sensor;
+HTS221 *temp_sensor1;
+
+
+
+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 false;
+        }
+    
+    /* Configure sensor */
+    InitStructure.OutputDataRate = HTS221_ODR_12_5Hz;
+
+    if(ht_sensor->Init(&InitStructure) != HUM_TEMP_OK)
+    {
+        delete ht_sensor;
+        ht_sensor = NULL;
+        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 false;
+    }
+      
+    /* 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 false;
+    }
+            
+    /* 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 false;
+    }
+            
+    /* 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 Init_LSM303C_MAG(LSM303C_MAG_Sensor* magnetometer)
+{
+    uint8_t id = 0;
+    uint8_t error = 0;
+    
+    error = magnetometer->ReadID(&id);
+    
+    /* Check presence */
+    if(error)
+    {
+        EPRINT("LSM303C_MAG Not detected!\r\n");
+        delete magnetometer;
+        magnetometer = NULL;
+        return false;
+    }
+    
+    if (id != I_AM_LSM303C_MAG)
+    {
+        EPRINT("This is not a LSM303C_MAG (0x02X != 0x02X)\r\n", id, I_AM_LSM303C_MAG);
+        delete magnetometer;
+        magnetometer = NULL;
+        return false;
+    }
+    
+    if(magnetometer->Init(NULL))
+    {
+        return false;
+    }
+    
+    if(magnetometer->Set_M_ODR(1.250))
+    {
+        return false;
+    }
+    
+    if(magnetometer->Enable())
+    {
+        return false;
+    }
+    
+    return true;
+}
+
+bool Init_LSM303C_ACC(LSM303C_ACC_Sensor* accelerometer)
+{
+    uint8_t id = 0;
+    uint8_t error = 0;
+    
+    error = accelerometer->ReadID(&id);
+    
+    /* Check presence */
+    if(error)
+    {
+        EPRINT("LSM303C_ACC Not detected!\r\n");
+        delete accelerometer;
+        accelerometer = NULL;
+        return false;
+    }
+    
+    if (id != I_AM_LSM303C_ACC)
+    {
+        EPRINT("This is not a LSM303C_ACC (0x02X != 0x02X)\r\n", id, I_AM_LSM303C_ACC);
+        delete accelerometer;
+        accelerometer = NULL;
+        return false;
+    }
+    
+    if(accelerometer->Init(NULL))
+    {
+        return false;
+    }
+    
+    if(accelerometer->Set_X_ODR(10))
+    {
+        return false;
+    }
+    
+    if(accelerometer->Enable())
+    {
+        return false;
+    }
+    
+    return true;
+}
+
+// Cal method
+#define CALL_METH(obj, meth, param, ret) ((obj == NULL) ?       \
+                      ((*(param) = (ret)), 0) : \
+                      ((obj)->meth(param))      \
+                      )
+              
+__inline int32_t float2_to_int(float v)
+{
+    return (int32_t)(v*100);
+}
+
+bool mag_get_value(int32_t* buf)
+{
+#if (_MAG_EN_ == 0)
+    return simul_sensor_value(buf, 3, -1900, 1900);
+#elif (_MAG_EN_ == 1)
+    return CALL_METH(magnetometer, Get_M_Axes, buf, 0)? true : false;
+#else 
+    return false;
+#endif
+}
+
+bool acc_get_value(int32_t* buf)
+{
+#if (_ACC_EN_ == 0)
+    return simul_sensor_value(buf, 3, -1900, 1900);
+#elif (_ACC_EN_ == 1)
+    return CALL_METH(accelerometer, Get_X_Axes, buf, 0)? true : false;
+#else 
+    return false;
+#endif
+}
+
+bool gyr_get_value(int32_t* buf)
+{
+#if (_GYR_EN_ == 0)
+    return simul_sensor_value(buf, 3, -40000, 40000);
+#elif (_GYR_EN_ == 1)
+    return CALL_METH(gyroscope, Get_G_Axes, buf, 0)? true : false;
+#else 
+    return false;
+#endif
+}
+
+bool pre_get_value(int32_t* buf)
+{
+#if (_PRE_EN_ == 0)
+    return simul_sensor_value(buf, 1, 96000, 104000);
+#elif (_PRE_EN_ == 1)
+    bool err;
+    float tmp;
+    err = CALL_METH(pressure_sensor, GetPressure, &tmp, 0.0f)? true : false;
+    buf[0] = float2_to_int(tmp);
+    return err;
+#else 
+    return false;
+#endif
+}
+
+bool hum_get_value(int32_t* buf)
+{
+#if (_HUM_EN_ == 0)
+    return simul_sensor_value(buf, 1, 1000, 9000);
+#elif (_HUM_EN_ == 1)
+    bool err;
+    float tmp;
+    err = CALL_METH(humidity_sensor, GetHumidity, &tmp, 0.0f)? true : false;
+    buf[0] = float2_to_int(tmp);
+    return err;
+#else
+    return false;
+#endif
+}
+
+bool tem1_get_value(int32_t* buf)
+{
+#if (_TEM1_EN_ == 0)
+    return simul_sensor_value(buf, 1, 1100, 3900);
+#elif (_TEM1_EN_ == 1)
+    bool err;
+    float tmp;
+    err = CALL_METH(temp_sensor1, GetTemperature, &tmp, 0.0f)? true : false;
+    buf[0] = float2_to_int(tmp);    
+    return err;
+#else
+    return false;
+#endif
+}
+
+bool tem2_get_value(int32_t* buf)
+{
+#if (_TEM2_EN_ == 0)
+    return simul_sensor_value(buf, 1, 5100, 10100);
+#elif (_TEM2_EN_ == 1)
+    bool err;
+    float tmp;
+    err = CALL_METH(temp_sensor2, GetFahrenheit, &tmp, 0.0f)? true : false;
+    buf[0] = float2_to_int(tmp);
+    return err;
+#else
+    return false;
+#endif
+}
+
+bool light_get_value(int32_t* buf)
+{
+#if (_LIGHT_EN_ == 0)
+    return simul_sensor_value(buf, 1, 0, 100);
+#elif (_LIGHT_EN_ == 1) && defined(SENSOR_LIGHT_MEAS) && defined(SENSOR_LIGHT_EN)
+    float tmp;
+    g_light_en_l = 0;
+    Thread::wait(10);
+    tmp = g_light_meas;
+    g_light_en_l = 1;
+    buf[0] = float2_to_int(tmp);
+    return false;
+#else
+    return false;
+#endif
+}
\ No newline at end of file