void processAccGyroMeasurements(const uint8_t *buffer) { Axis3f accScaled; // Note the ordering to correct the rotated 90ยบ IMU coordinate system int16_t ay = (((int16_t) buffer[0]) << 8) | buffer[1]; int16_t ax = (((int16_t) buffer[2]) << 8) | buffer[3]; int16_t az = (((int16_t) buffer[4]) << 8) | buffer[5]; int16_t gy = (((int16_t) buffer[8]) << 8) | buffer[9]; int16_t gx = (((int16_t) buffer[10]) << 8) | buffer[11]; int16_t gz = (((int16_t) buffer[12]) << 8) | buffer[13]; #ifdef GYRO_BIAS_LIGHT_WEIGHT gyroBiasFound = processGyroBiasNoBuffer(gx, gy, gz, &gyroBias); #else gyroBiasFound = processGyroBias(gx, gy, gz, &gyroBias); #endif if (gyroBiasFound) { processAccScale(ax, ay, az); } sensors.gyro.x = -(gx - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG; sensors.gyro.y = (gy - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG; sensors.gyro.z = (gz - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG; applyAxis3fLpf((lpf2pData*)(&gyroLpf), &sensors.gyro); accScaled.x = -(ax) * SENSORS_G_PER_LSB_CFG / accScale; accScaled.y = (ay) * SENSORS_G_PER_LSB_CFG / accScale; accScaled.z = (az) * SENSORS_G_PER_LSB_CFG / accScale; sensorsAccAlignToGravity(&accScaled, &sensors.acc); applyAxis3fLpf((lpf2pData*)(&accLpf), &sensors.acc); }
static void sensorsTask(void *param) { systemWaitStart(); Axis3f accScaled; /* wait an additional second the keep bus free * this is only required by the z-ranger, since the * configuration will be done after system start-up */ //vTaskDelayUntil(&lastWakeTime, M2T(1500)); while (1) { if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY)) { sensorData.interruptTimestamp = imuIntTimestamp; /* get data from chosen sensors */ sensorsGyroGet(&gyroRaw); sensorsAccelGet(&accelRaw); /* calibrate if necessary */ #ifdef GYRO_BIAS_LIGHT_WEIGHT gyroBiasFound = processGyroBiasNoBuffer(gyroRaw.x, gyroRaw.y, gyroRaw.z, &gyroBias); #else gyroBiasFound = processGyroBias(gyroRaw.x, gyroRaw.y, gyroRaw.z, &gyroBias); #endif if (gyroBiasFound) { processAccScale(accelRaw.x, accelRaw.y, accelRaw.z); } /* Gyro */ sensorData.gyro.x = (gyroRaw.x - gyroBias.x) * SENSORS_BMI088_DEG_PER_LSB_CFG; sensorData.gyro.y = (gyroRaw.y - gyroBias.y) * SENSORS_BMI088_DEG_PER_LSB_CFG; sensorData.gyro.z = (gyroRaw.z - gyroBias.z) * SENSORS_BMI088_DEG_PER_LSB_CFG; applyAxis3fLpf((lpf2pData*)(&gyroLpf), &sensorData.gyro); /* Acelerometer */ accScaled.x = accelRaw.x * SENSORS_BMI088_G_PER_LSB_CFG / accScale; accScaled.y = accelRaw.y * SENSORS_BMI088_G_PER_LSB_CFG / accScale; accScaled.z = accelRaw.z * SENSORS_BMI088_G_PER_LSB_CFG / accScale; sensorsAccAlignToGravity(&accScaled, &sensorData.acc); applyAxis3fLpf((lpf2pData*)(&accLpf), &sensorData.acc); } if (isBarometerPresent) { static uint8_t baroMeasDelay = SENSORS_DELAY_BARO; if (--baroMeasDelay == 0) { uint8_t sensor_comp = BMP3_PRESS | BMP3_TEMP; struct bmp3_data data; baro_t* baro388 = &sensorData.baro; /* Temperature and Pressure data are read and stored in the bmp3_data instance */ bmp3_get_sensor_data(sensor_comp, &data, &bmp388Dev); sensorsScaleBaro(baro388, data.pressure, data.temperature); baroMeasDelay = baroMeasDelayMin; } } xQueueOverwrite(accelerometerDataQueue, &sensorData.acc); xQueueOverwrite(gyroDataQueue, &sensorData.gyro); if (isBarometerPresent) { xQueueOverwrite(barometerDataQueue, &sensorData.baro); } xSemaphoreGive(dataReady); } }