Complete sensor demo.
Dependencies: modem_ref_helper CRC X_NUCLEO_IKS01A1 DebouncedInterrupt
sensors.cpp
- Committer:
- Jeej
- Date:
- 2018-10-11
- Revision:
- 14:8fd5405f7ab2
- Parent:
- 6:c17f7cbdeb1a
- Child:
- 15:1271f3566b98
File content as of revision 14:8fd5405f7ab2:
#include "mbed.h" #include "WizziDebug.h" #include "sensors.h" #include "hwcfg.h" #include "simul.h" #include "sensors_cfg.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, 1000); #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] = (int32_t)(tmp*1000); return false; #else return false; #endif }