Hello Dear Forum Members,
I’m using FreeRTOS in my Raspberry Pi Pico.
I have 2 sensors in my system which use the same I2C bus.
I’m having a problem while two tasks running. (I think they both trying to access the same I2C Bus at the same time.)
I found that using “Mutex” can fix the problem but I couldn’t understand how to use Mutex.
Can somebody help me with this?
I tried to add Mutex into my code but after adding Mutex the code stops running after 10 seconds.
Yes, you need to use a Mutex (or an equivalent). Generally I put the mutex in the low level driver so I know that all calls will check it.
Make sure you check the return value of the mutex call, and only use the device if the mutex acquire succeeds. Also make sure you give the mutex when done. If the code that has the mutex might try to take it again within the protection, you will need a recursive mutex.
Thanks for the answer.
I tried to add Mutex to my code but my pico started to freeze after the code starts running.
void tof(void *param) {
(void)param;
Wire.setSDA(4);
Wire.setSCL(5);
Wire.begin();
Serial1.begin(115200);
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(50 / portTICK_PERIOD_MS);
Serial1.println(F("Both in reset mode...(pins are low)"));
Serial1.println(F("Starting..."));
setID();
while (1) {
if (xSemaphoreCreateRecursiveMutex() != NULL) {
if (xSemaphoreTake(mutex, 0) == pdTRUE) {
oku();
vTaskDelay(100 / portTICK_PERIOD_MS);
}
xSemaphoreGive(mutex);
} else {
}
}
}
void mputask(void *param) {
(void)param;
while (1) {
if (xSemaphoreCreateRecursiveMutex() != NULL) {
if (xSemaphoreTake(mutex, 0) == pdTRUE) {
vTaskDelay(500 / portTICK_PERIOD_MS);
// if programming failed, don't try to do anything
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial1.print("MPU1\t");
Serial1.print(ypr[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr[2] * 180 / M_PI);
#endif
}
/////////////////////////////////////////////////////////////////////////// 2. MPU veri alımı ////////////////////////////////////////////////////////////////////////////
Serial1.print("\r");
if (!dmp2Ready) return;
if (mpu2.dmpGetCurrentFIFOPacket(fifoBuffer2)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
mpu2.dmpGetGravity(&gravity2, &q2);
mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
Serial1.print("MPU2\t");
Serial1.print(ypr2[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr2[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr2[2] * 180 / M_PI);
#endif
}
}
xSemaphoreGive(mutex);
}
}
}
void loop() {
// put your main code here, to run repeatedly:
}
You don’t keep on creating the mutex in your task. You should create it ONCE, preferably before starting the scheduler with the handle available to everyone that needs it. You then just take it when you need it. Also, you should only give it if the take succeeded. Giving a mutex you don’t have is an error that can trigger an assert.
I edited my code again, ( I deleted the unnecessary part of the code)
Now MPU6050s works but there is no output about TOF sensors.
static SemaphoreHandle_t mutex;
void setup() {
mutex = xSemaphoreCreateMutex();
xTaskCreate(tof, "TOF", 1024, nullptr, 1, nullptr);
xTaskCreate(blink, "BLINK", 128, nullptr, 1, nullptr);
xTaskCreate(mputask, "MPUTASK", 1024, nullptr, 2, nullptr);
void tof(void *param) {
(void)param;
Wire.setSDA(4);
Wire.setSCL(5);
Wire.begin();
Serial1.begin(115200);
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(50 / portTICK_PERIOD_MS);
Serial1.println(F("Both in reset mode...(pins are low)"));
Serial1.println(F("Starting..."));
setID();
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) { // mutex kilidini almaya çalış
oku();
vTaskDelay(100 / portTICK_PERIOD_MS);
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
}
}
void mputask(void *param) {
(void)param;
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
vTaskDelay(500 / portTICK_PERIOD_MS);
// if programming failed, don't try to do anything
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial1.print("MPU1\t");
Serial1.print(ypr[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr[2] * 180 / M_PI);
#endif
}
/////////////////////////////////////////////////////////////////////////// 2. MPU veri alımı ////////////////////////////////////////////////////////////////////////////
Serial1.print("\r");
if (!dmp2Ready) return;
if (mpu2.dmpGetCurrentFIFOPacket(fifoBuffer2)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
mpu2.dmpGetGravity(&gravity2, &q2);
mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
Serial1.print("MPU2\t");
Serial1.print(ypr2[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr2[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr2[2] * 180 / M_PI);
#endif
}
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
}
}
void loop() {
// put your main code here, to run repeatedly:
}
I don’t see any code in tof to actually output anything from the loop.
You also should put the vTaskDelay call OUTSIDE the mutex hold, or the MPU task will just give the semaphore then immediately grab it back since it is higher priority than the tof task.
I think this worked but it still freezes sometimes at the beginning.
Let me upload my whole code for better analysis.
#include "Adafruit_VL53L0X.h"
#include <FreeRTOS.h>
#include <task.h>
#include "semphr.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define LOX1_ADDRESS 0x30
#define LOX2_ADDRESS 0x31
#define SHT_LOX1 6
#define SHT_LOX2 9
#define TIMER_INTERRUPT_DEBUG 1
#define _TIMERINTERRUPT_LOGLEVEL_ 4
#include "RPi_Pico_TimerInterrupt.h"
uint8_t sayac = 0;
RPI_PICO_Timer ITimer1(1);
#define TIMER1_INTERVAL_MS 1000
bool TimerHandler1(struct repeating_timer *t) {
(void)t;
static bool toggle1 = false;
#if (TIMER_INTERRUPT_DEBUG > 0)
//timer interrupt toggles pin outputPin1
sayac++;
#endif
return true;
}
Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;
MPU6050 mpu;
MPU6050 mpu2(0x69);
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
#define INTERRUPT_PIN2 3
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
bool blinkState = false;
bool dmpReady = false;
bool dmp2Ready = false;
uint8_t mpuIntStatus;
uint8_t mpu2IntStatus;
uint8_t devStatus;
uint8_t dev2Status;
uint16_t packetSize;
uint16_t packetSize2;
uint8_t fifoBuffer[64];
uint8_t fifoBuffer2[64];
// orientation/motion vars
Quaternion q, q2;
VectorInt16 aa, aa2;
VectorInt16 aaReal, aaReal2;
VectorInt16 aaWorld, aaWorld2;
VectorFloat gravity, gravity2;
float euler[3], euler2[3];
float ypr[3], ypr2[3];
volatile bool mpuInterrupt = false;
volatile bool mpu2Interrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void dmpDataReady2() {
mpu2Interrupt = true;
}
static SemaphoreHandle_t mutex;
void setup() {
mutex = xSemaphoreCreateMutex();
xTaskCreate(tof, "TOF", 1024, nullptr, 1, nullptr);
xTaskCreate(blink, "BLINK", 128, nullptr, 1, nullptr);
xTaskCreate(mputask, "MPUTASK", 1024, nullptr, 2, nullptr);
pinMode(SHT_LOX1, OUTPUT);
pinMode(SHT_LOX2, OUTPUT);
Serial1.println(F("Shutdown pins inited..."));
if (ITimer1.attachInterruptInterval(TIMER1_INTERVAL_MS * 1000, TimerHandler1)) {
Serial1.print(F("Starting ITimer1 OK, millis() = "));
Serial1.println(millis());
#if (TIMER_INTERRUPT_DEBUG > 1)
Serial1.print(F("OutputPin1 = "));
Serial1.print(outputPin1);
#endif
} else
Serial1.println(F("Can't set ITimer1. Select another freq. or timer"));
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
}
void blink(void *param) {
(void)param;
pinMode(LED_BUILTIN, OUTPUT);
while (1) {
digitalWrite(LED_BUILTIN, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
digitalWrite(LED_BUILTIN, HIGH);
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
void read_dual_sensors() {
lox1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
lox2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
// print sensor one reading
Serial1.print(F("TOF1: "));
if (measure1.RangeStatus != 4) { // if not out of range
Serial1.print(measure1.RangeMilliMeter);
} else {
Serial1.print(F("Out of range"));
}
Serial1.print(F(" "));
// print sensor two reading
Serial1.print(F("TOF2: "));
if (measure2.RangeStatus != 4) {
Serial1.print(measure2.RangeMilliMeter);
} else {
Serial1.print(F("Out of range"));
}
Serial1.println();
}
void init() {
setID();
Serial1.println("initlendim");
}
void oku() {
if ((Serial1.available() && Serial1.read())) {
int h;
while (h = Serial1.available() > 0) {
while (h--) Serial1.read();
}
sayac = 0;
init();
}
if (sayac <= 20) {
vTaskDelay(10 / portTICK_PERIOD_MS);
read_dual_sensors();
}
if (sayac > 20) {
lox1.stopMeasurement();
lox2.stopMeasurement();
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
}
}
void setID() {
// all reset
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(10 / portTICK_PERIOD_MS);
// all unreset
digitalWrite(SHT_LOX1, HIGH);
digitalWrite(SHT_LOX2, HIGH);
vTaskDelay(10 / portTICK_PERIOD_MS);
// activating LOX1 and resetting LOX2
digitalWrite(SHT_LOX1, HIGH);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
// initing LOX1
if (!lox1.begin(LOX1_ADDRESS, 0, &Wire)) {
Serial1.println(F("Failed to boot first VL53L0X"));
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
setID();
}
vTaskDelay(10 / portTICK_PERIOD_MS);
// activating LOX2
digitalWrite(SHT_LOX2, HIGH);
vTaskDelay(100 / portTICK_PERIOD_MS);
//initing LOX2
if (!lox2.begin(LOX2_ADDRESS, 0, &Wire)) {
Serial1.println(F("Failed to boot second VL53L0X"));
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
setID();
}
vTaskDelay(10 / portTICK_PERIOD_MS);
}
void tof(void *param) {
(void)param;
Wire.setSDA(4);
Wire.setSCL(5);
Wire.begin();
Serial1.begin(115200);
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(50 / portTICK_PERIOD_MS);
Serial1.println(F("Both in reset mode...(pins are low)"));
Serial1.println(F("Starting..."));
setID();
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) { // mutex kilidini almaya çalış
oku();
vTaskDelay(100 / portTICK_PERIOD_MS);
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
}
}
void mputask(void *param) {
(void)param;
Serial1.println(F("Initializing I2C devices..."));
mpu.initialize();
mpu2.initialize();
pinMode(INTERRUPT_PIN, INPUT);
pinMode(INTERRUPT_PIN2, INPUT);
Serial1.println(F("Testing device connections..."));
Serial1.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial1.println(mpu2.testConnection() ? F("MPU6050 2 connection successful") : F("MPU6050 2 connection failed"));
Serial1.println(F("\nSend any character to begin DMP programming and demo: "));
//while (Serial1.available() && Serial1.read())
// ;
//while (!Serial1.available())
// ;
//while (Serial1.available() && Serial1.read())
// ;
Serial1.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
dev2Status = mpu2.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
mpu2.setXGyroOffset(220);
mpu2.setYGyroOffset(76);
mpu2.setZGyroOffset(-85);
mpu2.setZAccelOffset(1788);
if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
Serial1.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial1.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial1.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial1.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial1.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial1.print(F("DMP Initialization failed (code "));
Serial1.print(devStatus);
Serial1.println(F(")"));
}
if (dev2Status == 0) {
mpu2.CalibrateAccel(6);
mpu2.CalibrateGyro(6);
mpu2.PrintActiveOffsets();
Serial1.println(F("Enabling DMP..."));
mpu2.setDMPEnabled(true);
Serial1.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial1.print(digitalPinToInterrupt(INTERRUPT_PIN2));
Serial1.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN2), dmpDataReady2, RISING);
mpu2IntStatus = mpu2.getIntStatus();
Serial1.println(F("DMP ready! Waiting for first interrupt..."));
dmp2Ready = true;
packetSize2 = mpu2.dmpGetFIFOPacketSize();
} else {
Serial1.print(F("DMP Initialization failed (code "));
Serial1.print(dev2Status);
Serial1.println(F(")"));
}
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
vTaskDelay(500 / portTICK_PERIOD_MS);
// if programming failed, don't try to do anything
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial1.print("MPU1\t");
Serial1.print(ypr[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr[2] * 180 / M_PI);
#endif
}
/////////////////////////////////////////////////////////////////////////// 2. MPU veri alımı ////////////////////////////////////////////////////////////////////////////
Serial1.print("\r");
if (!dmp2Ready) return;
if (mpu2.dmpGetCurrentFIFOPacket(fifoBuffer2)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
mpu2.dmpGetGravity(&gravity2, &q2);
mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
Serial1.print("MPU2\t");
Serial1.print(ypr2[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr2[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr2[2] * 180 / M_PI);
#endif
}
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
}
}
void loop() {
// put your main code here, to run repeatedly:
}
You have a high priority task and low priority task contesting for same mutex - it is likely that the low priority one will starve. What is the purpose of the delays after obtaining mutex? If it is to give other task a chance to run, it needs to be after releasing mutex:
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) { // mutex kilidini almaya çalış
oku();
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
.
.
Serial1.print(ypr2[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr2[2] * 180 / M_PI);
#endif
}
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
vTaskDelay(500 / portTICK_PERIOD_MS);
}
This delay is just for testing, it’s not existing in the code anymore.
For now, it seems okay, I’ll post to this topic later if anything comes up new.
Thanks!