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);
}
bool sensorsManufacturingTest(void)
{
bool testStatus = false;
Axis3i16 g;
Axis3i16 a;
Axis3f acc; // Accelerometer axis data in mG
float pitch, roll;
uint32_t startTick = xTaskGetTickCount();
testStatus = mpu6500SelfTest();
if (testStatus)
{
sensorsBiasObjInit(&gyroBiasRunning);
while (xTaskGetTickCount() - startTick < SENSORS_VARIANCE_MAN_TEST_TIMEOUT)
{
mpu6500GetMotion6(&a.y, &a.x, &a.z, &g.y, &g.x, &g.z);
if (processGyroBias(g.x, g.y, g.z, &gyroBias))
{
gyroBiasFound = true;
DEBUG_PRINT("Gyro variance test [OK]\n");
break;
}
}
if (gyroBiasFound)
{
acc.x = -(a.x) * SENSORS_G_PER_LSB_CFG;
acc.y = (a.y) * SENSORS_G_PER_LSB_CFG;
acc.z = (a.z) * SENSORS_G_PER_LSB_CFG;
// Calculate pitch and roll based on accelerometer. Board must be level
pitch = tanf(-acc.x/(sqrtf(acc.y*acc.y + acc.z*acc.z))) * 180/(float) M_PI;
roll = tanf(acc.y/acc.z) * 180/(float) M_PI;
if ((fabsf(roll) < SENSORS_MAN_TEST_LEVEL_MAX) && (fabsf(pitch) < SENSORS_MAN_TEST_LEVEL_MAX))
{
DEBUG_PRINT("Acc level test [OK]\n");
testStatus = true;
}
else
{
DEBUG_PRINT("Acc level test Roll:%0.2f, Pitch:%0.2f [FAIL]\n", (double)roll, (double)pitch);
testStatus = false;
}
}
else
{
DEBUG_PRINT("Gyro variance test [FAIL]\n");
testStatus = false;
}
}
return testStatus;
}
static void sensorsGyroCalibrate(BiasObj* gyro, uint8_t type) {
if (gyro->found == 0)
{
if (gyro->ongoing == 0)
{
sensorsBiasMalloc(gyro);
}
/* write directly into buffer */
sensorsGyroGet(gyro->bufPtr, type);
/* FIXME: for sensor deck v1 realignment has to be added her */
sensorsBiasBufPtrIncrement(gyro);
if (gyro->bufIsFull == 1)
{
if (processGyroBias(gyro))
sensorsBiasFree(gyro);
}
}
}
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);
}
}
