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