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);
}
}