static void imu_sensor_magneto_data_handle(void* data)
{
/*mag data*/
if(LSM303AGR_MAG_Get_Magnetic(sensor_data.mag) != imu_status_ok)
{
printf("read sensor error\n");
return ;
}
run_after_delay(imu_sensor_read_data_from_fifo, NULL,1);
}
/*
* Callback to handle the ACC event.
* It must be registered to be called at interrupt time.
*
* This is triggered by FIFO WTM interrupt
*/
static void LSM303AGR_ACC_sample_Callback(u8_t intID)
{
LSM303AGR_ACC_sample_calls++;
u8_t num = 0, cnt = 0;
/*
* Read ACC samples from FIFO
*/
LSM303AGR_ACC_R_FifoSamplesAvail(&num);
while (num-- > 0)
LSM303AGR_ACC_Get_Acceleration(Acceleration_mG[cnt++]);
/*
* Read MAG samples from FIFO
*/
LSM303AGR_MAG_Get_Magnetic(Magnetic_mGa);
}
/* Test Acquisition of sensor samples */
static void Loop_Test_Sample_Aquisition(void)
{
/* init accelerometer */
if (init_LSM303AGR_acc(LSM303AGR_ACC_FS_2G, LSM303AGR_ACC_ODR_DO_100Hz, FIFO_THSLD) < 0)
return; /* handle error */
/*
* Set mag to lowest ODR, because in this example we read one
* mag samples every FIFO-threshold acc samples.
*/
if (init_LSM303AGR_mag(LSM303AGR_MAG_ODR_10Hz) < 0)
return; /* handle error */
#if (TEST_WITH_WTM_INTERRUPT == 1)
while(1) {
/* Event will be handled in driver callback */
}
#else
/*
* Read samples in polling mode (no int)
*/
while(1)
{
LSM303AGR_ACC_WTM_t wtm;
u8_t num = 0, cnt = 0;
/* Read LSM303AGR output only if new ACC values are available */
LSM303AGR_ACC_R_WatermarkLevel(&wtm);
if (wtm == LSM303AGR_ACC_WTM_OVERFLOW) {
LSM303AGR_ACC_R_FifoSamplesAvail(&num);
while (num-- > 0)
LSM303AGR_ACC_Get_Acceleration(Acceleration_mG[cnt++]);
}
/* Read LSM303AGR also the MAG sample */
LSM303AGR_MAG_Get_Magnetic(Magnetic_mGa);
}
#endif
}
/*fifo read*/
imu_status_t imu_sensor_read_data_from_fifo(void)
{
uint16_t fifo_threthold = 2046;
int16_t pData[3] = {0};
uint8_t tempReg[2] = {0, 0};
sensor_data_type_t flag;
imu_sensor_data_t sensor_data = {0.0,0.0,0.0};
printf("read io\n");
flag = TYPE_ACC_DATA;
/*mag data*/
if(LSM303AGR_MAG_Get_Magnetic(sensor_data.mag) != imu_status_ok)
{
printf("read sensor error\n");
return imu_status_fail;
}
while(fifo_threthold > 3) {
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, 2) != imu_status_ok)
{
printf("read sensor error\n");
return imu_status_fail;
}
// printf("ok\n");
pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, 2) != imu_status_ok)
{
printf("read sensor error\n");
return imu_status_fail;
}
pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_MEMS_ADDRESS, LSM6DS3_XG_FIFO_DATA_OUT_L, 2) != imu_status_ok)
{
printf("read sensor error\n");
return imu_status_fail;
}
pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
if(flag == TYPE_ACC_DATA) {
/*acc data*/
sensor_data.acc[0] = (float)(pData[0] * sensor_data_sensitivity.acc_sensitivity);
sensor_data.acc[1] = (float)(pData[1] * sensor_data_sensitivity.acc_sensitivity);
sensor_data.acc[2] = (float)(pData[2] * sensor_data_sensitivity.acc_sensitivity);
flag = TYPE_GYRO_DATA;
}else if (flag == TYPE_GYRO_DATA) {
/*gyro data*/
sensor_data.gyro[0] = (float)(pData[0] * sensor_data_sensitivity.gyro_sensitivity);
sensor_data.gyro[1] = (float)(pData[1] * sensor_data_sensitivity.gyro_sensitivity);
sensor_data.gyro[2] = (float)(pData[2] * sensor_data_sensitivity.gyro_sensitivity);
BSP_LED_Toggle(LED0);
on_imu_sensor_data(&sensor_data);
flag = TYPE_ACC_DATA;
}
fifo_threthold-=3;
}
return imu_status_ok;
}
