static void sensorsTask(void *param)
{
systemWaitStart();
while (1)
{
if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
{
// data is ready to be read
uint8_t dataLen = (uint8_t) (14 + (isMagnetometerPresent ? 6 : 0) + (isBarometerPresent ? 5 : 0));
i2cdevRead(I2C3_DEV, MPU6500_ADDRESS_AD0_HIGH, MPU6500_RA_ACCEL_XOUT_H, dataLen, buffer);
// these functions process the respective data and queue it on the output queues
processAccGyroMeasurements(&(buffer[0]));
if (isMagnetometerPresent) { processMagnetometerMeasurements(&(buffer[14])); }
if (isBarometerPresent) { processBarometerMeasurements(&(buffer[isMagnetometerPresent ? 20 : 14])); }
vTaskSuspendAll(); // ensure all queues are populated at the same time
xQueueOverwrite(accelerometerDataQueue, &sensors.acc);
xQueueOverwrite(gyroDataQueue, &sensors.gyro);
if (isMagnetometerPresent) { xQueueOverwrite(magnetometerDataQueue, &sensors.mag); }
if (isBarometerPresent) { xQueueOverwrite(barometerDataQueue, &sensors.baro); }
xTaskResumeAll();
}
}
}
void crtpRxTask(void *param)
{
CRTPPacket p;
static unsigned int droppedPacket=0;
while (true)
{
if (link != &nopLink)
{
if (!link->receivePacket(&p))
{
if (queues[p.port])
{
// The queue is only 1 long, so if the last packet hasn't been processed, we just replace it
xQueueOverwrite(queues[p.port], &p);
}
else
{
droppedPacket++;
}
if (callbacks[p.port])
callbacks[p.port](&p); //Dangerous?
}
}
else
{
vTaskDelay(M2T(10));
}
}
}
static void sensorsTask(void *param)
{
systemWaitStart();
sensorsSetupSlaveRead();
while (1)
{
if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
{
sensorData.interruptTimestamp = imuIntTimestamp;
// data is ready to be read
uint8_t dataLen = (uint8_t) (SENSORS_MPU6500_BUFF_LEN +
(isMagnetometerPresent ? SENSORS_MAG_BUFF_LEN : 0) +
(isBarometerPresent ? SENSORS_BARO_BUFF_LEN : 0));
i2cdevRead(I2C3_DEV, MPU6500_ADDRESS_AD0_HIGH, MPU6500_RA_ACCEL_XOUT_H, dataLen, buffer);
// these functions process the respective data and queue it on the output queues
processAccGyroMeasurements(&(buffer[0]));
if (isMagnetometerPresent)
{
processMagnetometerMeasurements(&(buffer[SENSORS_MPU6500_BUFF_LEN]));
}
if (isBarometerPresent)
{
processBarometerMeasurements(&(buffer[isMagnetometerPresent ?
SENSORS_MPU6500_BUFF_LEN + SENSORS_MAG_BUFF_LEN : SENSORS_MPU6500_BUFF_LEN]));
}
xQueueOverwrite(accelerometerDataQueue, &sensorData.acc);
xQueueOverwrite(gyroDataQueue, &sensorData.gyro);
if (isMagnetometerPresent)
{
xQueueOverwrite(magnetometerDataQueue, &sensorData.mag);
}
if (isBarometerPresent)
{
xQueueOverwrite(barometerDataQueue, &sensorData.baro);
}
// Unlock stabilizer task
xSemaphoreGive(dataReady);
}
}
}
void SensorReading(void *pvParams)
{
uint8 buff[7];
uint8 bfd[15];
// Initialize timer, 1Hz period
TickType_t readFrequency = 1000;
TickType_t wakeUpTime = xTaskGetTickCount();
// Sensor variables
double Bfield[3];
int tmp;
while(1)
{
// Read magnetic field
xEventGroupSetBits(torqueSignalEG, 1<<1);
vTaskDelay(50);
taskENTER_CRITICAL();
/*buff[1] = i2cRegisterRead(4, 20);
buff[2] = i2cRegisterRead(4, 21);
buff[3] = i2cRegisterRead(4, 22);
buff[4] = i2cRegisterRead(4, 23);
buff[5] = i2cRegisterRead(4, 24);
buff[6] = i2cRegisterRead(4, 25);*/
taskEXIT_CRITICAL();
Bfield[0] = ((double)((buff[1]<<8) + buff[2] - 32768))*((double)1.52587890625E-9);//0.00005/32768
Bfield[1] = ((double)((buff[3]<<8) + buff[4] - 32768))*((double)1.52587890625E-9);
Bfield[2] = ((double)((buff[5]<<8) + buff[6] - 32768))*((double)1.52587890625E-9);
/*sprintf(bfd, "%.7f,", Bfield[0]);
sciSend(scilinREG, 10, bfd);
sprintf(bfd, "%.7f,", Bfield[1]);
sciSend(scilinREG, 10, bfd);
sprintf(bfd, "%.7f", Bfield[2]);
sciSend(scilinREG, 10, bfd);
bfd[0] = '\n';
sciSend(scilinREG, 1, bfd);*/
vTaskDelay(50);
xEventGroupSetBits(torqueSignalEG, 1<<2);
// Dispatch read values
xQueueOverwrite(SensorDataQ, Bfield);
vTaskDelayUntil(&wakeUpTime, readFrequency);
}
}
void vTaskCommander (void *pvParameters)
{
QueueSetMemberHandle_t xActivatedMember;
RadioLinkData radioData;
IMUData imuData;
LogData log;
CommanderData commanderData;
static bool isArmed = false;
static float lastTick = xTaskGetTickCount();
//(const float kp, const float ki, const float kd, const float intLimit)
PidObject yawRatePid(0.3f, 0, 0.1f, 1);
PidObject pitchPid (2.0f, 10.0f, 0.13f, 10.0f);
PidObject rollPid (2.0f, 10.0f, 0.13f, 10.0f);
static bool imuReady = false; // We need at last one IMU data packet to start work.
static bool radioReady = false; // We need at last one radio data packet to start work.
while(1)
{
xActivatedMember = xQueueSelectFromSet(TheStabilizerQueueSet, 10);
if (xActivatedMember == TheRadioCommandsQueue)
{
xQueueReceive(TheRadioCommandsQueue, &radioData, 0 );
radioReady = true;
}
if (xActivatedMember == TheIMUDataQueue)
{
xQueueReceive(TheIMUDataQueue, &imuData, 0 );
imuReady = true;
}
else
{
// failure!
continue;
}
if (!imuReady || !radioReady)
{
// Wait for first complete data.
continue;
}
if (!isArmed)
{
// We can arm only if IMU and radio ready.
if (Arm(commanderData))
{
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
continue;
}
// Arm done.
isArmed = true;
}
TheLedInfo->Y(true);
// dT calculation
const float currentTick = xTaskGetTickCount();
const float dT = (currentTick - lastTick) / 1000.0f; // Seconds
lastTick = currentTick;
if (dT == 0)
{
continue; // Skip first iteration.
}
if (radioData.Throttle <= 2 )
{
yawRatePid.Reset();
pitchPid.Reset();
rollPid.Reset();
commanderData.Throttle = 0;
commanderData.Pitch = 0;
commanderData.Roll = 0;
commanderData.Yaw = 0;
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
continue;
}
// Convert radio to angles
const float angle_max = radians(30.0f);
const float yawRateDesired = map(radioData.Yaw, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float pitchDesired = -map(radioData.Pitch, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float rollDesired = map(radioData.Roll, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float throttleDesired = map(radioData.Throttle, 0.0f, 100.0f, 10.0f, 100.0f); // [10, 100] percent of motor power
// Feed PID regulators
const float yawCorrection = yawRatePid.Update( yawRateDesired, imuData.Yaw, dT, true); // radians
const float pitchCorrection = pitchPid .Update( pitchDesired, imuData.Pitch, dT, true); // radians
const float rollCorrection = rollPid .Update( rollDesired, imuData.Roll, dT, true); // radians
// Calculate motors power
const float pm = 30.0f * throttleDesired / 100.0f; // Multiplier for Pitch
const float rm = 30.0f * throttleDesired / 100.0f; // Multiplier for Roll
const float ym = 5.0f; // Multiplier for Yaw
const float pitchK = pitchCorrection * pm; // [-1, 1] pitch correction
const float rollK = rollCorrection * rm; // [-1, 1] roll correction
const float yawK = cosf(yawCorrection); // [-1, 1] yaw correction
commanderData.Throttle = throttleDesired;
commanderData.Pitch = pitchK;
commanderData.Roll = rollK;
commanderData.Yaw = yawK;
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
TheGlobalData.DBG_PID_YAW = yawCorrection;
TheGlobalData.DBG_PID_PITCH = pitchCorrection;
TheGlobalData.DBG_PID_ROLL = rollCorrection;
TheGlobalData.DBG_CO_YAW = yawK;
TheGlobalData.DBG_CO_PITCH = pitchK;
TheGlobalData.DBG_CO_ROLL = rollK;
TheLedInfo->Y(false);
/*
log.Timer = xTaskGetTickCount();
log.InputThrottle = radioData.Throttle;
log.InputYaw = radioData.Yaw;
log.InputPitch = radioData.Pitch;
log.InputRoll = radioData.Roll;
log.Yaw = imuData.EulierAngles.Z;
log.Roll = imuData.EulierAngles.Y;
log.Pitch = imuData.EulierAngles.X;
*/
// Testing telemetry.
//xQueueOverwrite( TheLogQueue, &log );
// Process Data!
}
vTaskDelete(NULL);
}
static void prvQueueOverwriteTask( void *pvParameters )
{
QueueHandle_t xTaskQueue;
const UBaseType_t uxQueueLength = 1;
uint32_t ulValue, ulStatus = pdPASS, x;
/* The parameter is not used. */
( void ) pvParameters;
/* Create the queue. xQueueOverwrite() should only be used on queues that
have a length of 1. */
xTaskQueue = xQueueCreate( uxQueueLength, ( UBaseType_t ) sizeof( uint32_t ) );
configASSERT( xTaskQueue );
for( ;; )
{
/* The queue is empty. Writing to the queue then reading from the queue
should return the item written. */
ulValue = 10;
xQueueOverwrite( xTaskQueue, &ulValue );
ulValue = 0;
xQueueReceive( xTaskQueue, &ulValue, qoDONT_BLOCK );
if( ulValue != 10 )
{
ulStatus = pdFAIL;
}
/* Now try writing to the queue several times. Each time the value
in the queue should get overwritten. */
for( x = 0; x < qoLOOPS; x++ )
{
/* Write to the queue. */
xQueueOverwrite( xTaskQueue, &x );
/* Check the value in the queue is that written, even though the
queue was not necessarily empty. */
xQueuePeek( xTaskQueue, &ulValue, qoDONT_BLOCK );
if( ulValue != x )
{
ulStatus = pdFAIL;
}
/* There should always be one item in the queue. */
if( uxQueueMessagesWaiting( xTaskQueue ) != uxQueueLength )
{
ulStatus = pdFAIL;
}
}
/* Empty the queue again. */
xQueueReceive( xTaskQueue, &ulValue, qoDONT_BLOCK );
if( uxQueueMessagesWaiting( xTaskQueue ) != 0 )
{
ulStatus = pdFAIL;
}
if( ulStatus != pdFAIL )
{
/* Increment a counter to show this task is still running without
error. */
ulLoopCounter++;
}
#if( configUSE_PREEMPTION == 0 )
taskYIELD();
#endif
}
}
void kalmanTask(void *p) {
kalmanHandler_t * aileronKalmanHandler = initializeAileronKalman();
kalmanHandler_t * elevatorKalmanHandler = initializeElevatorKalman();
/* -------------------------------------------------------------------- */
/* Vector for 1-state measurement */
/* -------------------------------------------------------------------- */
vector_float * measurement_1_state = vector_float_alloc(1, 0);
/* -------------------------------------------------------------------- */
/* px4flow speed measurement noise matrix (Q) (1-state) */
/* -------------------------------------------------------------------- */
matrix_float * px4flow_Q_matrix_1_state = matrix_float_alloc(1, 1);
matrix_float_set(px4flow_Q_matrix_1_state, 1, 1, KALMAN_Q);
/* -------------------------------------------------------------------- */
/* px4flow speed C matrix (transfer measurements -> states) */
/* -------------------------------------------------------------------- */
matrix_float * px4flow_C_matrix_1_state = matrix_float_alloc(1, NUMBER_OF_STATES_ELEVATOR);
matrix_float_set(px4flow_C_matrix_1_state, 1, 1, 0);
matrix_float_set(px4flow_C_matrix_1_state, 1, 2, 1);
matrix_float_set(px4flow_C_matrix_1_state, 1, 3, 0);
matrix_float_set(px4flow_C_matrix_1_state, 1, 4, 0);
matrix_float_set(px4flow_C_matrix_1_state, 1, 5, 0);
/* -------------------------------------------------------------------- */
/* Messages between tasks */
/* -------------------------------------------------------------------- */
comm2kalmanMessage_t comm2kalmanMessage;
kalman2mpcMessage_t kalman2mpcMessage;
kalman2commMessage_t kalman2commMesasge;
resetKalmanMessage_t resetKalmanMessage;
while (1) {
if (xQueueReceive(resetKalmanQueue, &resetKalmanMessage, 0)) {
// reset state vectors
vector_float_set_zero(elevatorKalmanHandler->states);
vector_float_set_zero(aileronKalmanHandler->states);
// set the default position
vector_float_set(elevatorKalmanHandler->states, 1, resetKalmanMessage.elevatorPosition);
vector_float_set(aileronKalmanHandler->states, 1, resetKalmanMessage.aileronPosition);
// reset the covariance matrices
matrix_float_set_identity(elevatorKalmanHandler->covariance);
matrix_float_set_identity(aileronKalmanHandler->covariance);
}
if (xQueueReceive(setKalmanQueue, &resetKalmanMessage, 0)) {
// set the position
vector_float_set(elevatorKalmanHandler->states, 1, resetKalmanMessage.elevatorPosition);
vector_float_set(aileronKalmanHandler->states, 1, resetKalmanMessage.aileronPosition);
// reset the covariance of the postion
matrix_float_set(elevatorKalmanHandler->covariance, 1, 1, 1);
matrix_float_set(aileronKalmanHandler->covariance, 1, 1, 1);
}
if (xQueueReceive(comm2kalmanQueue, &comm2kalmanMessage, 0)) {
/* -------------------------------------------------------------------- */
/* Compute elevator kalman */
/* -------------------------------------------------------------------- */
// set the input vector
vector_float_set(elevatorKalmanHandler->input, 1, comm2kalmanMessage.elevatorInput);
// set the measurement vector
vector_float_set(measurement_1_state, 1, comm2kalmanMessage.elevatorSpeed);
// set pointers to measurement related matrices
elevatorKalmanHandler->measurement = measurement_1_state;
elevatorKalmanHandler->C_matrix = px4flow_C_matrix_1_state;
elevatorKalmanHandler->Q_matrix = px4flow_Q_matrix_1_state;
kalmanIteration(elevatorKalmanHandler);
/* -------------------------------------------------------------------- */
/* Compute aileron kalman */
/* -------------------------------------------------------------------- */
// set the input vector
vector_float_set(aileronKalmanHandler->input, 1, comm2kalmanMessage.aileronInput);
// set the measurement vector
vector_float_set(measurement_1_state, 1, comm2kalmanMessage.aileronSpeed);
// set pointers to measurement related matrices
aileronKalmanHandler->measurement = measurement_1_state;
aileronKalmanHandler->C_matrix = px4flow_C_matrix_1_state;
aileronKalmanHandler->Q_matrix = px4flow_Q_matrix_1_state;
kalmanIteration(aileronKalmanHandler);
/* -------------------------------------------------------------------- */
/* Create a message for mpcTask */
/* -------------------------------------------------------------------- */
memcpy(&kalman2mpcMessage.elevatorData, elevatorKalmanHandler->states->data, NUMBER_OF_STATES_ELEVATOR*sizeof(float));
memcpy(&kalman2mpcMessage.aileronData, aileronKalmanHandler->states->data, NUMBER_OF_STATES_AILERON*sizeof(float));
xQueueOverwrite(kalman2mpcQueue, &kalman2mpcMessage);
/* -------------------------------------------------------------------- */
/* Create a message for commTask */
/* -------------------------------------------------------------------- */
memcpy(&kalman2commMesasge.elevatorData, elevatorKalmanHandler->states->data, NUMBER_OF_STATES_ELEVATOR*sizeof(float));
memcpy(&kalman2commMesasge.aileronData, aileronKalmanHandler->states->data, NUMBER_OF_STATES_AILERON*sizeof(float));
xQueueOverwrite(kalman2commQueue, &kalman2commMesasge);
}
}
}
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);
}
}
