WizziLab / Mbed 2 deprecated D7A_1x_TRAINING

Demo fro training

Dependencies:   X_NUCLEO_IKS01A1 d7a_1x mbed-rtos mbed wizzi-utils

sensors.cpp@0:429446fe396d, 2016-11-21 (annotated)

Committer:
mikl_andre
Date:
Mon Nov 21 07:24:34 2016 +0000
Revision:
0:429446fe396d
Initial Revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikl_andre 0:429446fe396d 1 #include "mbed.h"
mikl_andre 0:429446fe396d 2 #include "dbg.h"
mikl_andre 0:429446fe396d 3 #include "sensors.h"
mikl_andre 0:429446fe396d 4 #include "simul.h"
mikl_andre 0:429446fe396d 5
mikl_andre 0:429446fe396d 6 #define CALL_METH(obj, meth, param, ret) ((obj == NULL) ? \
mikl_andre 0:429446fe396d 7 ((*(param) = (ret)), 0) : \
mikl_andre 0:429446fe396d 8 ((obj)->meth(param)) \
mikl_andre 0:429446fe396d 9 )
mikl_andre 0:429446fe396d 10
mikl_andre 0:429446fe396d 11 LIS3MDL *magnetometer;
mikl_andre 0:429446fe396d 12 LSM6DS0 *accelerometer;
mikl_andre 0:429446fe396d 13 LSM6DS0 *gyroscope;
mikl_andre 0:429446fe396d 14 LPS25H *pressure_sensor;
mikl_andre 0:429446fe396d 15 LPS25H *temp_sensor2;
mikl_andre 0:429446fe396d 16 HTS221 *humidity_sensor;
mikl_andre 0:429446fe396d 17 HTS221 *temp_sensor1;
mikl_andre 0:429446fe396d 18
mikl_andre 0:429446fe396d 19 __inline int32_t float2_to_int(float v)
mikl_andre 0:429446fe396d 20 {
mikl_andre 0:429446fe396d 21 return (int32_t)(v*100);
mikl_andre 0:429446fe396d 22 }
mikl_andre 0:429446fe396d 23
mikl_andre 0:429446fe396d 24 bool Init_HTS221(HTS221* ht_sensor)
mikl_andre 0:429446fe396d 25 {
mikl_andre 0:429446fe396d 26 uint8_t ht_id = 0;
mikl_andre 0:429446fe396d 27 HUM_TEMP_InitTypeDef InitStructure;
mikl_andre 0:429446fe396d 28
mikl_andre 0:429446fe396d 29 /* Check presence */
mikl_andre 0:429446fe396d 30 if((ht_sensor->ReadID(&ht_id) != HUM_TEMP_OK) ||
mikl_andre 0:429446fe396d 31 (ht_id != I_AM_HTS221))
mikl_andre 0:429446fe396d 32 {
mikl_andre 0:429446fe396d 33 delete ht_sensor;
mikl_andre 0:429446fe396d 34 ht_sensor = NULL;
mikl_andre 0:429446fe396d 35 return true;
mikl_andre 0:429446fe396d 36 }
mikl_andre 0:429446fe396d 37
mikl_andre 0:429446fe396d 38 /* Configure sensor */
mikl_andre 0:429446fe396d 39 InitStructure.OutputDataRate = HTS221_ODR_12_5Hz;
mikl_andre 0:429446fe396d 40
mikl_andre 0:429446fe396d 41 if(ht_sensor->Init(&InitStructure) != HUM_TEMP_OK)
mikl_andre 0:429446fe396d 42 {
mikl_andre 0:429446fe396d 43 return false;
mikl_andre 0:429446fe396d 44 }
mikl_andre 0:429446fe396d 45
mikl_andre 0:429446fe396d 46 return true;
mikl_andre 0:429446fe396d 47 }
mikl_andre 0:429446fe396d 48
mikl_andre 0:429446fe396d 49 bool Init_LIS3MDL(LIS3MDL* magnetometer)
mikl_andre 0:429446fe396d 50 {
mikl_andre 0:429446fe396d 51 uint8_t m_id = 0;
mikl_andre 0:429446fe396d 52 MAGNETO_InitTypeDef InitStructure;
mikl_andre 0:429446fe396d 53
mikl_andre 0:429446fe396d 54 /* Check presence */
mikl_andre 0:429446fe396d 55 if((magnetometer->ReadID(&m_id) != MAGNETO_OK) ||
mikl_andre 0:429446fe396d 56 (m_id != I_AM_LIS3MDL_M))
mikl_andre 0:429446fe396d 57 {
mikl_andre 0:429446fe396d 58 delete magnetometer;
mikl_andre 0:429446fe396d 59 magnetometer = NULL;
mikl_andre 0:429446fe396d 60 return true;
mikl_andre 0:429446fe396d 61 }
mikl_andre 0:429446fe396d 62
mikl_andre 0:429446fe396d 63 /* Configure sensor */
mikl_andre 0:429446fe396d 64 InitStructure.M_FullScale = LIS3MDL_M_FS_4;
mikl_andre 0:429446fe396d 65 InitStructure.M_OperatingMode = LIS3MDL_M_MD_CONTINUOUS;
mikl_andre 0:429446fe396d 66 InitStructure.M_XYOperativeMode = LIS3MDL_M_OM_HP;
mikl_andre 0:429446fe396d 67 InitStructure.M_OutputDataRate = LIS3MDL_M_DO_80;
mikl_andre 0:429446fe396d 68
mikl_andre 0:429446fe396d 69 if(magnetometer->Init(&InitStructure) != MAGNETO_OK)
mikl_andre 0:429446fe396d 70 {
mikl_andre 0:429446fe396d 71 return false;
mikl_andre 0:429446fe396d 72 }
mikl_andre 0:429446fe396d 73
mikl_andre 0:429446fe396d 74 return true;
mikl_andre 0:429446fe396d 75 }
mikl_andre 0:429446fe396d 76
mikl_andre 0:429446fe396d 77 bool Init_LPS25H(LPS25H* pt_sensor)
mikl_andre 0:429446fe396d 78 {
mikl_andre 0:429446fe396d 79 uint8_t p_id = 0;
mikl_andre 0:429446fe396d 80 PRESSURE_InitTypeDef InitStructure;
mikl_andre 0:429446fe396d 81
mikl_andre 0:429446fe396d 82 /* Check presence */
mikl_andre 0:429446fe396d 83 if((pt_sensor->ReadID(&p_id) != PRESSURE_OK) ||
mikl_andre 0:429446fe396d 84 (p_id != I_AM_LPS25H))
mikl_andre 0:429446fe396d 85 {
mikl_andre 0:429446fe396d 86 delete pt_sensor;
mikl_andre 0:429446fe396d 87 pt_sensor = NULL;
mikl_andre 0:429446fe396d 88 return true;
mikl_andre 0:429446fe396d 89 }
mikl_andre 0:429446fe396d 90
mikl_andre 0:429446fe396d 91 /* Configure sensor */
mikl_andre 0:429446fe396d 92 InitStructure.OutputDataRate = LPS25H_ODR_1Hz;
mikl_andre 0:429446fe396d 93 InitStructure.BlockDataUpdate = LPS25H_BDU_CONT;
mikl_andre 0:429446fe396d 94 InitStructure.DiffEnable = LPS25H_DIFF_DISABLE;
mikl_andre 0:429446fe396d 95 InitStructure.SPIMode = LPS25H_SPI_SIM_4W;
mikl_andre 0:429446fe396d 96 InitStructure.PressureResolution = LPS25H_P_RES_AVG_8;
mikl_andre 0:429446fe396d 97 InitStructure.TemperatureResolution = LPS25H_T_RES_AVG_8;
mikl_andre 0:429446fe396d 98
mikl_andre 0:429446fe396d 99 if(pt_sensor->Init(&InitStructure) != PRESSURE_OK)
mikl_andre 0:429446fe396d 100 {
mikl_andre 0:429446fe396d 101 return false;
mikl_andre 0:429446fe396d 102 }
mikl_andre 0:429446fe396d 103
mikl_andre 0:429446fe396d 104 return true;
mikl_andre 0:429446fe396d 105 }
mikl_andre 0:429446fe396d 106
mikl_andre 0:429446fe396d 107 bool Init_LSM6DS0(LSM6DS0* gyro_lsm6ds0)
mikl_andre 0:429446fe396d 108 {
mikl_andre 0:429446fe396d 109 IMU_6AXES_InitTypeDef InitStructure;
mikl_andre 0:429446fe396d 110 uint8_t xg_id = 0;
mikl_andre 0:429446fe396d 111
mikl_andre 0:429446fe396d 112 /* Check presence */
mikl_andre 0:429446fe396d 113 if((gyro_lsm6ds0->ReadID(&xg_id) != IMU_6AXES_OK) ||
mikl_andre 0:429446fe396d 114 (xg_id != I_AM_LSM6DS0_XG))
mikl_andre 0:429446fe396d 115 {
mikl_andre 0:429446fe396d 116 delete gyro_lsm6ds0;
mikl_andre 0:429446fe396d 117 gyro_lsm6ds0 = NULL;
mikl_andre 0:429446fe396d 118 return true;
mikl_andre 0:429446fe396d 119 }
mikl_andre 0:429446fe396d 120
mikl_andre 0:429446fe396d 121 /* Configure sensor */
mikl_andre 0:429446fe396d 122 InitStructure.G_FullScale = 2000.0f; /* 2000DPS */
mikl_andre 0:429446fe396d 123 InitStructure.G_OutputDataRate = 119.0f; /* 119HZ */
mikl_andre 0:429446fe396d 124 InitStructure.G_X_Axis = 1; /* Enable */
mikl_andre 0:429446fe396d 125 InitStructure.G_Y_Axis = 1; /* Enable */
mikl_andre 0:429446fe396d 126 InitStructure.G_Z_Axis = 1; /* Enable */
mikl_andre 0:429446fe396d 127
mikl_andre 0:429446fe396d 128 InitStructure.X_FullScale = 2.0f; /* 2G */
mikl_andre 0:429446fe396d 129 InitStructure.X_OutputDataRate = 119.0f; /* 119HZ */
mikl_andre 0:429446fe396d 130 InitStructure.X_X_Axis = 1; /* Enable */
mikl_andre 0:429446fe396d 131 InitStructure.X_Y_Axis = 1; /* Enable */
mikl_andre 0:429446fe396d 132 InitStructure.X_Z_Axis = 1; /* Enable */
mikl_andre 0:429446fe396d 133
mikl_andre 0:429446fe396d 134 if(gyro_lsm6ds0->Init(&InitStructure) != IMU_6AXES_OK)
mikl_andre 0:429446fe396d 135 {
mikl_andre 0:429446fe396d 136 return false;
mikl_andre 0:429446fe396d 137 }
mikl_andre 0:429446fe396d 138
mikl_andre 0:429446fe396d 139 return true;
mikl_andre 0:429446fe396d 140 }
mikl_andre 0:429446fe396d 141
mikl_andre 0:429446fe396d 142 bool mag_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 143 {
mikl_andre 0:429446fe396d 144 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 145 return simul_sensor_value(buf, 3, -1900, 1900);
mikl_andre 0:429446fe396d 146 #else
mikl_andre 0:429446fe396d 147 return CALL_METH(magnetometer, Get_M_Axes, buf, 0)? true : false;
mikl_andre 0:429446fe396d 148 #endif
mikl_andre 0:429446fe396d 149 }
mikl_andre 0:429446fe396d 150
mikl_andre 0:429446fe396d 151 bool acc_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 152 {
mikl_andre 0:429446fe396d 153 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 154 return simul_sensor_value(buf, 3, -1900, 1900);
mikl_andre 0:429446fe396d 155 #else
mikl_andre 0:429446fe396d 156 return CALL_METH(accelerometer, Get_X_Axes, buf, 0)? true : false;
mikl_andre 0:429446fe396d 157 #endif
mikl_andre 0:429446fe396d 158 }
mikl_andre 0:429446fe396d 159
mikl_andre 0:429446fe396d 160 bool gyr_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 161 {
mikl_andre 0:429446fe396d 162 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 163 return simul_sensor_value(buf, 3, -40000, 40000);
mikl_andre 0:429446fe396d 164 #else
mikl_andre 0:429446fe396d 165 return CALL_METH(gyroscope, Get_G_Axes, buf, 0)? true : false;
mikl_andre 0:429446fe396d 166 #endif
mikl_andre 0:429446fe396d 167 }
mikl_andre 0:429446fe396d 168
mikl_andre 0:429446fe396d 169 bool pre_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 170 {
mikl_andre 0:429446fe396d 171 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 172 return simul_sensor_value(buf, 1, 96000, 104000);
mikl_andre 0:429446fe396d 173 #else
mikl_andre 0:429446fe396d 174 bool err;
mikl_andre 0:429446fe396d 175 float tmp;
mikl_andre 0:429446fe396d 176 err = CALL_METH(pressure_sensor, GetPressure, &tmp, 0.0f)? true : false;
mikl_andre 0:429446fe396d 177 buf[0] = float2_to_int(tmp);
mikl_andre 0:429446fe396d 178 return err;
mikl_andre 0:429446fe396d 179 #endif
mikl_andre 0:429446fe396d 180 }
mikl_andre 0:429446fe396d 181
mikl_andre 0:429446fe396d 182 bool hum_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 183 {
mikl_andre 0:429446fe396d 184 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 185 return simul_sensor_value(buf, 1, 1000, 9000);
mikl_andre 0:429446fe396d 186 #else
mikl_andre 0:429446fe396d 187 bool err;
mikl_andre 0:429446fe396d 188 float tmp;
mikl_andre 0:429446fe396d 189 err = CALL_METH(humidity_sensor, GetHumidity, &tmp, 0.0f)? true : false;
mikl_andre 0:429446fe396d 190 buf[0] = float2_to_int(tmp);
mikl_andre 0:429446fe396d 191 return err;
mikl_andre 0:429446fe396d 192 #endif
mikl_andre 0:429446fe396d 193 }
mikl_andre 0:429446fe396d 194
mikl_andre 0:429446fe396d 195 bool tem1_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 196 {
mikl_andre 0:429446fe396d 197 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 198 return simul_sensor_value(buf, 1, 1100, 3900);
mikl_andre 0:429446fe396d 199 #else
mikl_andre 0:429446fe396d 200 bool err;
mikl_andre 0:429446fe396d 201 float tmp;
mikl_andre 0:429446fe396d 202 err = CALL_METH(temp_sensor1, GetTemperature, &tmp, 0.0f)? true : false;
mikl_andre 0:429446fe396d 203 buf[0] = float2_to_int(tmp);
mikl_andre 0:429446fe396d 204 return err;
mikl_andre 0:429446fe396d 205 #endif
mikl_andre 0:429446fe396d 206 }
mikl_andre 0:429446fe396d 207
mikl_andre 0:429446fe396d 208 bool tem2_get_value(int32_t* buf)
mikl_andre 0:429446fe396d 209 {
mikl_andre 0:429446fe396d 210 #if _SENSORS_SIMU_
mikl_andre 0:429446fe396d 211 return simul_sensor_value(buf, 1, 5100, 10100);
mikl_andre 0:429446fe396d 212 #else
mikl_andre 0:429446fe396d 213 bool err;
mikl_andre 0:429446fe396d 214 float tmp;
mikl_andre 0:429446fe396d 215 err = CALL_METH(temp_sensor2, GetFahrenheit, &tmp, 0.0f)? true : false;
mikl_andre 0:429446fe396d 216 buf[0] = float2_to_int(tmp);
mikl_andre 0:429446fe396d 217 return err;
mikl_andre 0:429446fe396d 218 #endif
mikl_andre 0:429446fe396d 219 }