sensorsAccelCalibrate(BiasObj* accel, BiasObj* gyro, uint8_t type) {
  if (accel->found == 0)
    {
      if (accel->ongoing == 0)
        {
          sensorsBiasMalloc(accel);
        }
      /* write directly into buffer */
      sensorsAccelGet(accel->bufPtr, type);
      /* FIXME: for sensor deck v1 realignment has to be added her */
      sensorsBiasBufPtrIncrement(accel);
      if ( (accel->bufIsFull == 1) && (gyro->found == 1) )
        {
          processAccelBias(accel);
          switch(type) {
            case SENSORS_BMI160:
              accel->value.z -= SENSORS_BMI160_1G_IN_LSB;
              break;
            case SENSORS_BMI055:
              accel->value.z -= SENSORS_BMI055_1G_IN_LSB;
              break;
          }
          sensorsBiasFree(accel);
        }
    }
}
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);
  }
}