demo.c

00001 #include "mbed_wait_api.h"
00002 #include "demo.h"
00003 #include <math.h>   /* trunc */
00004 
00005 
00006 #define SAMPLE_ODR       ODR_LOW /*!< Accelerometer samples Output Data Rate */
00007 
00008 static uint8_t verbose = 1; /* Verbose output to UART terminal ON/OFF. */
00009 
00010 
00011 /**
00012  * @brief  Handle DEMO State Machine
00013  */
00014 typedef enum
00015 {
00016   STATUS_MEMS_INT1_DETECTED,
00017   STATUS_FIFO_DOWNLOAD,
00018   STATUS_SLEEP
00019 } DEMO_FIFO_STATUS;
00020 
00021 static void *LIS2DH12_0_handle = NULL;
00022 static volatile DEMO_FIFO_STATUS demoFifoStatus;
00023 
00024 static volatile uint8_t LPS22HH_DIL24_PRESENT = 0;
00025 static void *LPS22HH_P_0_handle = NULL;
00026 static void *LPS22HH_T_0_handle = NULL;
00027 
00028 uint8_t accel_get_num_samples(void)
00029 {
00030     uint16_t samplesToRead = 0;
00031 
00032     /* Get num of unread FIFO samples before reading data */
00033     if (BSP_ACCELERO_FIFO_Get_Num_Of_Samples_Ext(LIS2DH12_0_handle, &samplesToRead) == COMPONENT_ERROR)
00034     {
00035         return 0;
00036     }
00037 
00038     return samplesToRead;
00039 }
00040 
00041 static DrvStatusTypeDef Init_All_Sensors(void)
00042 {
00043     DrvStatusTypeDef ret;
00044 
00045     ret = BSP_ACCELERO_Init(LIS2DH12_0, &LIS2DH12_0_handle);
00046 
00047     if (BSP_TEMPERATURE_Init(LPS22HH_T_0, &LPS22HH_T_0_handle) == COMPONENT_OK)
00048     {
00049         LPS22HH_DIL24_PRESENT = 1;
00050     } else {
00051         c_log_printf("\e[31mlps22hh temp fail\e[0m\r\n");
00052         ret = COMPONENT_ERROR;
00053     }
00054 
00055     if (BSP_PRESSURE_Init(LPS22HH_P_0, &LPS22HH_P_0_handle) == COMPONENT_OK)
00056     {
00057         LPS22HH_DIL24_PRESENT = 1;
00058     } else {
00059         c_log_printf("\e[31mlps22hh pres fail\e[0m\r\n");
00060         ret = COMPONENT_ERROR;
00061     }
00062 
00063     return ret;
00064 }
00065 
00066 int accel_is_enabled(uint8_t* status)
00067 {
00068     DrvStatusTypeDef ret = BSP_ACCELERO_IsEnabled(LIS2DH12_0_handle, status);
00069     if (ret == COMPONENT_OK)
00070         return 0;
00071     else
00072         return -1;
00073 }
00074 
00075 int accel_enable(uint8_t en)
00076 {
00077     DrvStatusTypeDef ret;
00078     if (en)
00079         ret = BSP_ACCELERO_Sensor_Enable(LIS2DH12_0_handle);
00080     else
00081         ret = BSP_ACCELERO_Sensor_Disable(LIS2DH12_0_handle);
00082 
00083     if (ret == COMPONENT_OK)
00084         return 0;
00085     else
00086         return -1;
00087 }
00088 
00089 static DrvStatusTypeDef Enable_All_Sensors(void)
00090 {
00091     DrvStatusTypeDef ret = COMPONENT_NOT_IMPLEMENTED;
00092 
00093     if (LPS22HH_DIL24_PRESENT == 1)
00094     {
00095         BSP_TEMPERATURE_Sensor_Enable(LPS22HH_T_0_handle);
00096         ret = COMPONENT_OK;
00097     }
00098     if (LPS22HH_DIL24_PRESENT == 1)
00099     {
00100         BSP_PRESSURE_Sensor_Enable(LPS22HH_P_0_handle);
00101         ret = COMPONENT_OK;
00102     }
00103     
00104     return ret;
00105 }
00106 
00107 int demo_start()
00108 {
00109     if (Init_All_Sensors() == COMPONENT_ERROR)
00110     {
00111         c_log_printf("Init_All_Sensors fail\r\n");
00112         return -1;
00113     }
00114 
00115     if (Enable_All_Sensors() == COMPONENT_ERROR)
00116     {
00117         c_log_printf("Enable_All_Sensors fail\r\n");
00118         return -1;
00119     }
00120 
00121     /* Configure LIS2DH12 Sensor for the DEMO application */
00122     if (BSP_ACCELERO_Set_ODR(LIS2DH12_0_handle, SAMPLE_ODR) == COMPONENT_ERROR)
00123     {
00124         c_log_printf("Set_ODR fail\r\n");
00125         return -1;
00126     }
00127 
00128     /* Set FIFO_OVR on INT1 */
00129     if (BSP_ACCELERO_FIFO_Set_INT1_FIFO_Overrun_Ext(LIS2DH12_0_handle, LIS2DH12_ENABLE) == COMPONENT_ERROR)
00130     {
00131         c_log_printf("Set_INT1 fail\r\n");
00132         return -1;
00133     }
00134 
00135     demoFifoStatus = STATUS_SLEEP;
00136 
00137     return 0;
00138 }
00139 
00140 int lis2dh_mainloop()
00141 {
00142     uint8_t fifo_overrun_status;
00143 
00144     /* Handle DEMO State Machine */
00145     switch (demoFifoStatus)
00146     {
00147       case STATUS_MEMS_INT1_DETECTED:
00148 
00149         /* Check if FIFO is full */
00150         if (BSP_ACCELERO_FIFO_Get_Overrun_Status_Ext(LIS2DH12_0_handle, &fifo_overrun_status) == COMPONENT_ERROR)
00151         {
00152             c_log_printf("get_overrun_status fail\r\n");
00153 
00154             return LIS2DH_BSP_FAIL;
00155         }
00156 
00157         if (fifo_overrun_status == 1)
00158         {
00159           demoFifoStatus = STATUS_FIFO_DOWNLOAD;
00160         }
00161         else
00162         {
00163           demoFifoStatus = STATUS_SLEEP;
00164         }
00165         break;
00166 
00167       case STATUS_FIFO_DOWNLOAD:
00168         demoFifoStatus = STATUS_SLEEP;
00169         return LIS2DH_MAIN_READ_FIFO;
00170 
00171       case STATUS_SLEEP:
00172         /* Enter sleep mode */
00173         return LIS2DH_MAIN_SLEEP;
00174 
00175       default:
00176         return LIS2DH_FAIL_STATE;
00177     } // ..switch (demoFifoStatus)
00178 
00179     return LIS2DH_MAIN_NONE;
00180 }
00181 
00182 int lis2dh_set_fifo_mode()
00183 {
00184     /* Set FIFO mode to FIFO */
00185     if (BSP_ACCELERO_FIFO_Set_Mode_Ext(LIS2DH12_0_handle, LIS2DH12_FIFO_MODE) == COMPONENT_ERROR)
00186     {
00187       return -1;
00188     }
00189     return 0;
00190 }
00191 
00192 int lis2dh_set_fifo_bypass()
00193 {
00194     if (BSP_ACCELERO_FIFO_Set_Mode_Ext(LIS2DH12_0_handle, LIS2DH12_FIFO_BYPASS_MODE) == COMPONENT_ERROR)
00195         return -1;
00196 
00197     return 0;
00198 }
00199 
00200 void EXTI4_15_IRQHandler()
00201 {
00202     HAL_GPIO_EXTI_IRQHandler(M_INT1_O_PIN);
00203     //HAL_GPIO_EXTI_IRQHandler(M_INT2_O_PIN);
00204 }
00205 
00206 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
00207 {
00208     if (GPIO_Pin == M_INT1_O_PIN) {
00209         lis2dh_int1();
00210         demoFifoStatus = STATUS_MEMS_INT1_DETECTED;
00211     }
00212 }
00213 
00214 DrvStatusTypeDef lis2dh12_get_axes(SensorAxes_t *acceleration)
00215 {
00216   /* Read single FIFO data (acceleration in 3 axes) */
00217   return BSP_ACCELERO_Get_Axes(LIS2DH12_0_handle, acceleration);
00218 }
00219 
00220 static void FloatToInt(float in, displayFloatToInt_t *out_value, int32_t dec_prec)
00221 {
00222   if (in >= 0.0f)
00223   {
00224     out_value->sign = 0;
00225   }
00226   else
00227   {
00228     out_value->sign = 1;
00229     in = -in;
00230   }
00231 
00232   out_value->out_int = (int32_t)in;
00233   in = in - (float)(out_value->out_int);
00234   out_value->out_dec = (int32_t)trunc(in * pow(10, dec_prec));
00235 }
00236 
00237 static void Temperature_Sensor_Handler(void *handle, displayFloatToInt_t* outValue)
00238 {
00239   uint8_t who_am_i;
00240   float odr;
00241   uint8_t id;
00242   float temperature;
00243   uint8_t status;
00244 
00245   BSP_TEMPERATURE_Get_Instance(handle, &id);
00246 
00247   BSP_TEMPERATURE_IsInitialized(handle, &status);
00248 
00249   if (status == 1)
00250   {
00251     if (BSP_TEMPERATURE_Get_Temp(handle, &temperature) == COMPONENT_ERROR)
00252     {
00253       temperature = 0.0f;
00254     }
00255 
00256     //displayFloatToInt_t out_value;
00257     FloatToInt(temperature, outValue, 2);
00258     c_log_printf("TEMP[%d]: %c%d.%02d\r\n", (int)id, ((outValue->sign) ? '-' : '+'),
00259              (int)outValue->out_int, (int)outValue->out_dec);
00260 
00261     if (verbose == 1)
00262     {
00263       if (BSP_TEMPERATURE_Get_WhoAmI(handle, &who_am_i) == COMPONENT_ERROR)
00264       {
00265         c_log_printf("\e[31mtemp-WHO AM I address[%d]: ERROR\e[0m\r\n", id);
00266       }
00267       else
00268       {
00269         c_log_printf("temp-WHO AM I address[%d]: 0x%02X\r\n", id, who_am_i);
00270       }
00271 
00272       if (BSP_TEMPERATURE_Get_ODR(handle, &odr) == COMPONENT_ERROR)
00273       {
00274         c_log_printf("\e[31mtemp-ODR[%d]: ERROR\e[0m\r\n", id);
00275       }
00276       else
00277       {
00278         displayFloatToInt_t out_value;
00279         FloatToInt(odr, &out_value, 3);
00280         c_log_printf("ODR[%d]: %c%d.%03d Hz\r\n", (int)id, ((out_value.sign) ? '-' : '+'),
00281                  (int)out_value.out_int, (int)out_value.out_dec);
00282       }
00283     }
00284   }
00285 }
00286 
00287 static void Pressure_Sensor_Handler(void *handle, displayFloatToInt_t* val)
00288 {
00289   uint8_t who_am_i;
00290   float odr;
00291   uint8_t id;
00292   float pressure;
00293   uint8_t status;
00294 
00295   BSP_PRESSURE_Get_Instance(handle, &id);
00296 
00297   BSP_PRESSURE_IsInitialized(handle, &status);
00298 
00299   if (status == 1)
00300   {
00301     if (BSP_PRESSURE_Get_Press(handle, &pressure) == COMPONENT_ERROR)
00302     {
00303 
00304       c_log_printf("\e[31mGet_Press fail\e[0m\r\n", id);
00305       pressure = 0.0f;
00306     }
00307 
00308     //displayFloatToInt_t out_value;
00309     FloatToInt(pressure, val, 2);
00310     c_log_printf("PRESS[%d]: %c%d.%02d\r\n", (int)id, ((val->sign) ? '-' : '+'),
00311              (int)val->out_int, (int)val->out_dec);
00312 
00313     if (verbose == 1)
00314     {
00315       if (BSP_PRESSURE_Get_WhoAmI(handle, &who_am_i) == COMPONENT_ERROR)
00316       {
00317         c_log_printf("\e[31mpres-WHO AM I address[%d]: ERROR\e[0m\r\n", id);
00318       }
00319       else
00320       {
00321         c_log_printf("pres-WHO AM I address[%d]: 0x%02X\r\n", id, who_am_i);
00322       }
00323 
00324       if (BSP_PRESSURE_Get_ODR(handle, &odr) == COMPONENT_ERROR)
00325       {
00326         c_log_printf("\e[31mpres-ODR[%d]: ERROR\e[0m\r\n", id);
00327       }
00328       else
00329       {
00330         displayFloatToInt_t out_value;
00331         FloatToInt(odr, &out_value, 3);
00332         c_log_printf("ODR[%d]: %c%d.%03d Hz\r\n", (int)id, ((out_value.sign) ? '-' : '+'),
00333                  (int)out_value.out_int, (int)out_value.out_dec);
00334       }
00335 
00336     }
00337   }
00338 }
00339 
00340 void demo_sample_pressure(displayFloatToInt_t* x)
00341 {
00342     Pressure_Sensor_Handler(LPS22HH_P_0_handle, x);
00343 }
00344 
00345 void demo_sample_temp(displayFloatToInt_t* outValue)
00346 {
00347     Temperature_Sensor_Handler(LPS22HH_T_0_handle, outValue);
00348 }
00349 
00350