I am using the MPU9250 sensor with FreeRTOS on an Arduino IDE with a ESP32. The sensor readings work correctly when accessed directly in the loop() function, but fail to provide accurate values when accessed from a task created with FreeRTOS.
#include <Wire.h> // Library for I2C communication
#include <MPU9250.h> // Include the MPU9250 library
#include <SD.h> // Library for SD card handling
#include <SPI.h> // Library for SPI communication
#define SD_CS_PIN 5 // Define the chip select pin for the SD card
#define QUEUE_LENGTH 10 // Define the length of the queue for data
#define CALIBRATION false // Set to true to enable sensor calibration
MPU9250 mpu; // Create an instance of the MPU9250 class
File dataFile; // File object to handle SD card file operations
QueueHandle_t dataQueue; // Handle for the queue used to pass data between tasks
void setup() {
Serial.begin(115200); // Initialize serial communication at 115200 baud rate
// Initialize I2C communication
Wire.begin();
delay(2000); // Wait for 2 seconds to ensure the I2C bus is ready
// Initialize the MPU9250 sensor
if (!mpu.setup(0x68)) { // Change to your sensor's I2C address if needed
while (1) {
Serial.println("Failed to connect to MPU9250. Check the connection.");
delay(5000); // Wait for 5 seconds before retrying
}
}
Serial.println("MPU9250 found!");
// Initialize the SD card
while (!SD.begin(SD_CS_PIN)) {
Serial.println("Card initialization failed or not present");
delay(500); // Wait for 0.5 seconds before retrying
}
Serial.println("Card initialized.");
// Create a new file on the SD card
dataFile = SD.open("/data.txt", FILE_WRITE);
while (!dataFile) {
Serial.println("Failed to create file");
delay(500); // Wait for 0.5 seconds before retrying
}
// Create a queue to hold QUEUE_LENGTH strings of size 100
dataQueue = xQueueCreate(QUEUE_LENGTH, sizeof(char[100]));
while (dataQueue == NULL) {
Serial.println("Failed to create queue");
delay(500); // Wait for 0.5 seconds before retrying
}
// Perform calibration if needed
if (CALIBRATION) {
calibrateMPU();
}
// Create tasks
xTaskCreatePinnedToCore(
readMPUTask, // Task function
"ReadMPU", // Task name
10000, // Stack size in words
NULL, // Task parameter
1, // Task priority
NULL, // Task handle
0); // Core where the task should run
xTaskCreatePinnedToCore(
logDataTask, // Task function
"LogData", // Task name
10000, // Stack size in words
NULL, // Task parameter
1, // Task priority
NULL, // Task handle
1); // Core where the task should run
}
void loop() {
// Nothing to do here, tasks are running independently
}
void readMPUTask(void *pvParameters) {
char dataString[100]; // Buffer to store the formatted data string
while (1) {
// Check if the MPU9250 sensor has new data
if (mpu.update()) {
static uint32_t prev_ms = millis();
if (millis() > prev_ms + 100) { // Update interval of 100 ms
// Format the data into a string
snprintf(dataString, sizeof(dataString), "%f,%f,%f", mpu.getYaw(), mpu.getPitch(), mpu.getRoll());
prev_ms = millis();
Serial.println(dataString); // Print data to the serial monitor
// Send the data to the logging task
xQueueSend(dataQueue, &dataString, portMAX_DELAY);
}
}
// Delay before the next read
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
void logDataTask(void *pvParameters) {
char dataString[100]; // Buffer to receive data from the queue
while (1) {
// Receive data from the queue
if (xQueueReceive(dataQueue, &dataString, portMAX_DELAY)) {
// Write the data to the SD card
if (dataFile) {
dataFile.println(dataString);
dataFile.flush(); // Ensure data is written to the card
}
}
}
}
void calibrateMPU() {
// Start calibration for accelerometer and gyroscope
Serial.println("Accelerometer and gyroscope calibration will start in 5 seconds.");
Serial.println("Keep the device still on a flat plane.");
mpu.verbose(true);
delay(5000);
mpu.calibrateAccelGyro();
// Start calibration for magnetometer
Serial.println("Magnetometer calibration will start in 5 seconds.");
Serial.println("Move the device in a figure-eight pattern to complete.");
delay(5000);
mpu.calibrateMag();
// Display calibration parameters
print_calibration();
mpu.verbose(false);
}
void print_calibration() {
Serial.println("< Calibration Parameters >");
Serial.println("Accelerometer Bias [g]: ");
Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
Serial.println();
Serial.println("Gyroscope Bias [deg/s]: ");
Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
Serial.print(", ");
Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
Serial.println();
Serial.println("Magnetometer Bias [mG]: ");
Serial.print(mpu.getMagBiasX());
Serial.print(", ");
Serial.print(mpu.getMagBiasY());
Serial.print(", ");
Serial.print(mpu.getMagBiasZ());
Serial.println();
Serial.println("Magnetometer Scale []: ");
Serial.print(mpu.getMagScaleX());
Serial.print(", ");
Serial.print(mpu.getMagScaleY());
Serial.print(", ");
Serial.print(mpu.getMagScaleZ());
Serial.println();
}
OUTPUT on Serial Monitor with acess in task:
-7.510000,-0.000000,0.000000
-7.510000,-0.000000,0.000000
-7.510000,-0.000000,0.000000
...
