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();
uint32_t lastWakeTime = xTaskGetTickCount();
static BiasObj bmi160GyroBias;
static BiasObj bmi055GyroBias;
#ifdef SENSORS_TAKE_ACCEL_BIAS
static BiasObj bmi160AccelBias;
static BiasObj bmi055AccelBias;
#endif
Axis3i16 gyroPrim;
Axis3i16 accelPrim;
Axis3f accelPrimScaled;
Axis3i16 accelPrimLPF;
Axis3i32 accelPrimStoredFilterValues;
#ifdef LOG_SEC_IMU
Axis3i16 gyroSec;
Axis3i16 accelSec;
Axis3f accelSecScaled;
Axis3i16 accelSecLPF;
Axis3i32 accelSecStoredFilterValues;
#endif /* LOG_SEC_IMU */
/* 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)
{
vTaskDelayUntil(&lastWakeTime, F2T(SENSORS_READ_RATE_HZ));
/* calibrate if necessary */
if (!allSensorsAreCalibrated)
{
if (!bmi160GyroBias.found) {
sensorsGyroCalibrate(&bmi160GyroBias, SENSORS_BMI160);
#ifdef SENSORS_TAKE_ACCEL_BIAS
sensorsAccelCalibrate(&bmi160AccelBias,
&bmi160GyroBias, SENSORS_BMI160);
#endif
}
if (!bmi055GyroBias.found)
{
sensorsGyroCalibrate(&bmi055GyroBias, SENSORS_BMI055);
#ifdef SENSORS_TAKE_ACCEL_BIAS
sensorsAccelCalibrate(&bmi055AccelBias,
&bmi055GyroBias, SENSORS_BMI055);
#endif
}
if ( bmi160GyroBias.found && bmi055GyroBias.found
#ifdef SENSORS_TAKE_ACCEL_BIAS
&& bmi160AccelBias.found && bmi055AccelBias.found
#endif
)
{
// soundSetEffect(SND_CALIB);
DEBUG_PRINT("Sensor calibration [OK].\n");
ledseqRun(SYS_LED, seq_calibrated);
allSensorsAreCalibrated= true;
}
}
else {
/* get data from chosen sensors */
sensorsGyroGet(&gyroPrim, gyroPrimInUse);
sensorsAccelGet(&accelPrim, accelPrimInUse);
#ifdef LOG_SEC_IMU
sensorsGyroGet(&gyroSec, gyroSecInUse);
sensorsAccelGet(&accelSec, accelSecInUse);
#endif
/* FIXME: for sensor deck v1 realignment has to be added her */
switch(gyroPrimInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&sensors.gyro, &gyroPrim,
&bmi160GyroBias.value,
SENSORS_BMI160_DEG_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&sensors.gyro, &gyroPrim,
&bmi055GyroBias.value,
SENSORS_BMI055_DEG_PER_LSB_CFG);
break;
}
sensorsAccIIRLPFilter(&accelPrim, &accelPrimLPF,
&accelPrimStoredFilterValues,
(int32_t)sensorsAccLpfAttFactor);
switch(accelPrimInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF,
&bmi160AccelBias.value,
SENSORS_BMI160_G_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF,
&bmi055AccelBias.value,
SENSORS_BMI055_G_PER_LSB_CFG);
break;
}
sensorsAccAlignToGravity(&accelPrimScaled, &sensors.acc);
#ifdef LOG_SEC_IMU
switch(gyroSecInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&sensors.gyroSec, &gyroSec,
&bmi160GyroBias.value,
SENSORS_BMI160_DEG_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&sensors.gyroSec, &gyroSec,
&bmi055GyroBias.value,
SENSORS_BMI055_DEG_PER_LSB_CFG);
break;
}
sensorsAccIIRLPFilter(&accelSec, &accelSecLPF,
&accelSecStoredFilterValues,
(int32_t)sensorsAccLpfAttFactor);
switch(accelSecInUse) {
case SENSORS_BMI160:
sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF,
&bmi160AccelBias.value,
SENSORS_BMI160_G_PER_LSB_CFG);
break;
case SENSORS_BMI055:
sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF,
&bmi055AccelBias.value,
SENSORS_BMI055_G_PER_LSB_CFG);
break;
}
sensorsAccAlignToGravity(&accelSecScaled, &sensors.accSec);
#endif
}
if (isMagnetometerPresent)
{
static uint8_t magMeasDelay = SENSORS_DELAY_MAG;
if (--magMeasDelay == 0)
{
bmm150_read_mag_data(&bmm150Dev);
sensors.mag.x = bmm150Dev.data.x;
sensors.mag.y = bmm150Dev.data.y;
sensors.mag.z = bmm150Dev.data.z;
magMeasDelay = SENSORS_DELAY_MAG;
}
}
if (isBarometerPresent)
{
static uint8_t baroMeasDelay = SENSORS_DELAY_BARO;
static int32_t v_temp_s32;
static uint32_t v_pres_u32;
static baro_t* baro280 = &sensors.baro;
if (--baroMeasDelay == 0)
{
bmp280_read_pressure_temperature(&v_pres_u32, &v_temp_s32);
sensorsScaleBaro(baro280, (float)v_pres_u32, (float)v_temp_s32/100.0f);
baroMeasDelay = baroMeasDelayMin;
}
}
/* ensure all queues are populated at the same time */
vTaskSuspendAll();
xQueueOverwrite(accelPrimDataQueue, &sensors.acc);
xQueueOverwrite(gyroPrimDataQueue, &sensors.gyro);
#ifdef LOG_SEC_IMU
xQueueOverwrite(gyroSecDataQueue, &sensors.gyroSec);
xQueueOverwrite(accelSecDataQueue, &sensors.accSec);
#endif
if (isBarometerPresent)
{
xQueueOverwrite(baroPrimDataQueue, &sensors.baro);
}
if (isMagnetometerPresent)
{
xQueueOverwrite(magPrimDataQueue, &sensors.mag);
}
xTaskResumeAll();
}
}
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);
}
}