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