Exemplos de on_imu_sensor_data em C++ (Cpp)

Exemplo n.º 1

Arquivo: imu_sensor.c Projeto: summer20100514/STM32_Platform

static void imu_sensor_6axis_data_handle(void* data)
{
    uint8_t i;
    int16_t pData[6] = {0};

    for(i = number_tmp; i < DMA_READ_6AXIS_DEPTH-number_tmp ; i+=12) {
        /*gyro*/
        pData[0] = ((((int16_t)tempReg_sensor[1+i]) << 8) + (int16_t)tempReg_sensor[0+i]);
        pData[1] = ((((int16_t)tempReg_sensor[3+i]) << 8) + (int16_t)tempReg_sensor[2+i]);
        pData[2] = ((((int16_t)tempReg_sensor[5+i]) << 8) + (int16_t)tempReg_sensor[4+i]);
        /*acc*/
        pData[3] = ((((int16_t)tempReg_sensor[7+i]) << 8) + (int16_t)tempReg_sensor[6+i]);
        pData[4] = ((((int16_t)tempReg_sensor[9+i]) << 8) + (int16_t)tempReg_sensor[8+i]);
        pData[5] = ((((int16_t)tempReg_sensor[11+i]) << 8) + (int16_t)tempReg_sensor[10+i]);

        /*gyro data*/
        sensor_data.gyro[0] = (float)((pData[0] * sensor_data_sensitivity.gyro_sensitivity)/1000);
        sensor_data.gyro[1] = (float)((pData[1] * sensor_data_sensitivity.gyro_sensitivity)/1000);
        sensor_data.gyro[2] = (float)((pData[2] * sensor_data_sensitivity.gyro_sensitivity)/1000);

        /*acc data*/
        sensor_data.acc[0] = (float)(pData[3] * sensor_data_sensitivity.acc_sensitivity);
        sensor_data.acc[1] = (float)(pData[4] * sensor_data_sensitivity.acc_sensitivity);
        sensor_data.acc[2] = (float)(pData[5] * sensor_data_sensitivity.acc_sensitivity);

        on_imu_sensor_data(&sensor_data);
    }

}

Exemplo n.º 2

Arquivo: imu_sensor.c Projeto: jcook/STM32_Platform

/*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;
}