When using two sensors at the same time in an embedded system, I want to prevent performance loss

Question

Currently, I am using the mpu6050 and uhf rfid module simultaneously on the lolin d32 pro (esp32) development board.

MPU6050 has a sampling rate of 100hz. I hope RFID also has a sampling rate of at least 10hz.

Currently, I am conducting two separate tests using freeRTOS. Depending on the priority, the

performance reduction for each is severe.

For example, if the priority of the IMU sensor is high, the recognition range of the RFID is reduced by more than 10 times than the typical one.

Also, if i increase the priority of RFID, IMU data collection is not smooth.

Is it difficult to get good performance from both? Is there a way to maximize it?

I would appreciate it if you could let me know.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <SPIFFS.h>
#include <SPI.h>
#include <WiFi.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#define RXD2 27
#define TXD2 26
#define BUFFER_SIZE 128
#define PIEZO_PIN 12
#define SWITCH_PIN 14
#define RFID_SAMPLING_INTERVAL 100
#define RFID_COMMAND_DELAY 10

volatile bool isWifiConnected = false;
volatile bool isServerConnected = false;
volatile bool sendData = false;
volatile bool stopMeasurement = false;

MPU6050 mpu;

uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorFloat gravity;
float ypr[3];
volatile bool mpuInterrupt = false;
bool dmpReady = false;

byte buffer[BUFFER_SIZE];
int bufferIndex = 0;

#define MAX_DATA_BUFFER 500
float dataBuffer[MAX_DATA_BUFFER];
int dataBufferIndex = 0;

byte command_sp[]             = {0xBB, 0x00, 0x22, 0x00, 0x00, 0x22, 0x7E};
byte Set_transmitting_power[] = {0xBB, 0x00, 0xB6, 0x00, 0x02, 0x0A, 0x28, 0x8F, 0x7E}; 
byte set_demodul[]            = {0xBB, 0x00, 0xF0, 0x00, 0x04, 0x03, 0x06, 0x01, 0xB0, 0xAE, 0x7E}; 
byte auto_freq_hopping[]      = {0xBB, 0x00, 0xAD, 0x00, 0x01, 0xFF, 0xAD, 0x7E}; 
byte command_mp[]             = {0xBB, 0x00, 0x27, 0x00, 0x03, 0x22, 0x27, 0x10, 0x83, 0x7E};
byte command_mp_stop[]        = {0xBB, 0x00, 0x28, 0x00, 0x00, 0x28, 0x7E};
byte set_query[]              = {0xBB, 0x00, 0x0E, 0x00, 0x02, 0x10, 0x20, 0x40, 0x7E};

volatile bool isMultiPolling = false;
TimerHandle_t restartTimer;

const char* ssid = "";
const char* password = "l";
const char* serverIP = "";
const int serverPort = ;

WiFiClient client;

struct SensorData {
  enum DataType { IMU, RFID } type;
  union {
    struct {
      // float ax;
      // float ay;
      // float az;
      float anorm;
      float yaw;
    } imu;
    byte rfid[24];
  } data;
};

struct RFIDCommand {
    const byte* command;
    size_t length;
};

QueueHandle_t rfidCommandQueue;
QueueHandle_t rfidResponseQueue;

struct RFIDResponse {
    byte data[BUFFER_SIZE];
    size_t length;
};

QueueHandle_t sensorQueue;

volatile unsigned long lastRFIDPollTime = 0;
volatile bool isRFIDProcessing = false;

void IRAM_ATTR dmpDataReady() {
    mpuInterrupt = true;
}

void setup() {
    Wire.begin();
    Serial.begin(115200);

    pinMode(SWITCH_PIN, INPUT_PULLUP);
    pinMode(PIEZO_PIN, OUTPUT);

    if (!SPIFFS.begin(true)) {
        Serial.println("SPIFFS initialization failed");
        return;
    }

    Serial.println("Waiting for double-click to start...");
}

void loop() {
    static unsigned long lastButtonPressTime = 0;
    static unsigned int clickCount = 0;
    const unsigned int clickInterval = 500;

    int buttonState = digitalRead(SWITCH_PIN);

    if (buttonState == LOW) {
        unsigned long currentTime = millis();

        if (currentTime - lastButtonPressTime > 50) {
            clickCount++;
            lastButtonPressTime = currentTime;
        }

        while (digitalRead(SWITCH_PIN) == LOW) {
            delay(10);
        }
    }

    if (millis() - lastButtonPressTime > clickInterval) {
        if (clickCount == 2 && !isWifiConnected) {
            Serial.println("Double-click detected. Connecting to WiFi and server...");
            connectWiFiAndServer();
        } else if (clickCount == 3 && isServerConnected) {
            sendData = false;
            stopMeasurement = true;
            Serial.println("Triple-click detected. Stopping measurement.");
            if (client.connected()) {
                client.print("Stop measurement\n");
                client.flush();
            }
        }
        clickCount = 0;
    }
}

void connectWiFiAndServer() {
    WiFi.begin(ssid);
    while (WiFi.status() != WL_CONNECTED) {
        delay(1000);
        Serial.println("Connecting WIFI...");
    }
    Serial.println("WiFi connected");
    isWifiConnected = true;

    if (client.connect(serverIP, serverPort)) {
        Serial.println("server connected");
        client.print("server connected");
        client.flush();
        isServerConnected = true;

        tone(PIEZO_PIN, 262, 300);
        delay(300);
        tone(PIEZO_PIN, 294, 300);
        delay(300);
        tone(PIEZO_PIN, 330, 300);
        delay(300);
        noTone(PIEZO_PIN);

        initializeSensors();
        startMeasurement();
    } else {
        Serial.println("server connect failed");
        isServerConnected = false;
    }
}


void startMeasurement() {
    sendData = true;
    stopMeasurement = false;

    xTaskCreatePinnedToCore(taskRFIDCommand, "RFID Command", 4096, NULL, 2, NULL, 0);
    xTaskCreatePinnedToCore(taskRFIDResponse, "RFID Response", 4096, NULL, 3, NULL, 0);

    xTaskCreatePinnedToCore(taskMPU, "MPU Task", 4096, NULL, 2, NULL, 1);
    xTaskCreatePinnedToCore(taskSendData, "Send Data Task", 4096, NULL, 1, NULL, 1);
}

void taskMPU(void* parameter) {
    for (;;) {
        if (isServerConnected && sendData && !stopMeasurement) {
            readMpuData();
        }
        // vTaskDelay(MPU_INTERVAL / portTICK_PERIOD_MS);
    }
}
void taskRFIDCommand(void* parameter) {
    RFIDCommand spCommand = {command_sp, sizeof(command_sp)};
    
    for (;;) {
        if (isServerConnected && sendData && !stopMeasurement) {
            unsigned long currentTime = millis();
            
            if (currentTime - lastRFIDPollTime >= RFID_SAMPLING_INTERVAL) {
                isRFIDProcessing = true;
                
                Serial2.write(spCommand.command, spCommand.length);
                Serial.println("RFID send command");
                
                lastRFIDPollTime = currentTime;
            }
        }
        vTaskDelay(1);
    }
}

void taskSendData(void* parameter) {
    SensorData sensorData;
    for (;;) {
        if (xQueueReceive(sensorQueue, &sensorData, portMAX_DELAY) == pdTRUE) {
            if (client.connected()) {
                if (sensorData.type == SensorData::IMU) {
                    client.print("IMU:");
                    client.write((uint8_t*)&sensorData.data.imu.anorm, sizeof(float));
                    // client.write((uint8_t*)&sensorData.data.imu.ax, sizeof(float));
                    // client.write((uint8_t*)&sensorData.data.imu.ay, sizeof(float));
                    // client.write((uint8_t*)&sensorData.data.imu.az, sizeof(float));
                    client.write((uint8_t*)&sensorData.data.imu.yaw, sizeof(float));
                    client.print("\n");
                } else if (sensorData.type == SensorData::RFID) {
                    client.print("RFID:[");
                    for (int i = 0; i < 24; i++) {
                        char hex[3];
                        sprintf(hex, "%02X", sensorData.data.rfid[i]);
                        client.write(hex, 2);
                        if (i < 23) {
                            client.write(',');
                        }
                    }
                    client.print("]\n");
                }
                client.flush();
            }
        }
        taskYIELD();
    }
}

void taskRFIDResponse(void* parameter) {
    byte tempBuffer[BUFFER_SIZE];
    int tempBufferIndex = 0;
    const TickType_t xDelay = pdMS_TO_TICKS(10); // 10ms delay
    
    for (;;) {
        if (isServerConnected && sendData && !stopMeasurement) {
            unsigned long startCheckTime = millis();
            
            while (Serial2.available() && (millis() - startCheckTime < 50)) {
                // 버퍼에 데이터 읽기
                tempBuffer[tempBufferIndex] = Serial2.read();
                tempBufferIndex++;
                
                if (tempBufferIndex >= 24) {
                    if (tempBuffer[19] == 0x53) {
                        SensorData sensorData;
                        sensorData.type = SensorData::RFID;
                        memcpy(sensorData.data.rfid, tempBuffer, 24);
                        
                        xQueueSend(sensorQueue, &sensorData, 0);
                        Serial.println("RFID tag detected");
                        tone(PIEZO_PIN, 330, 300);
                        noTone(PIEZO_PIN);
                    }
                    tempBufferIndex = 0;
                    break;
                }
            }
        }
        vTaskDelay(xDelay);
    }
}

void initializeSensors() {
    Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);
    mpu.initialize();
    devStatus = mpu.dmpInitialize();

    Serial2.write(Set_transmitting_power, sizeof(Set_transmitting_power));
    Serial2.write(auto_freq_hopping, sizeof(auto_freq_hopping));
    Serial2.write(set_query, sizeof(set_query));

    sensorQueue = xQueueCreate(100, sizeof(SensorData));
    if (sensorQueue == NULL) {
      Serial.println("Error queue");
    }

    mpu.setXGyroOffset(-1);
    mpu.setYGyroOffset(-36);
    mpu.setZGyroOffset(183);
    mpu.setXAccelOffset(59);
    mpu.setYAccelOffset(-2205);
    mpu.setZAccelOffset(1902);

    if (devStatus == 0) {
        mpu.setDMPEnabled(true);
        attachInterrupt(digitalPinToInterrupt(SWITCH_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        Serial.println("DMP initialization failed");
    }
}

void readMpuData() {
    if (!dmpReady)
        return;

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);

        float ax = aaReal.x / 16384.0 * 9.81;
        float ay = aaReal.y / 16384.0 * 9.81;
        float az = aaReal.z / 16384.0 * 9.81;
        float yaw = ypr[0] * 180.0 / M_PI;

        float a_norm = sqrt(ax*ax + ay*ay + az*az);

        float data[4] = {a_norm, yaw};

        SensorData sensorData;
        sensorData.type = SensorData::IMU;
        sensorData.data.imu.anorm = a_norm;
        // sensorData.data.imu.ax = ax;
        // sensorData.data.imu.ay = ay;
        // sensorData.data.imu.az = az;
        sensorData.data.imu.yaw = yaw;
        Serial.println("MPU6050 get data");
        xQueueSend(sensorQueue, &sensorData, portMAX_DELAY);
    }
}

void readSensorData() {
    while (Serial2.available() && bufferIndex < BUFFER_SIZE) {
        buffer[bufferIndex++] = Serial2.read();
    }

    if (bufferIndex >= 24) {
        processSensorData();
        bufferIndex = 0;
    }
}

void processSensorData() {
    if (buffer[19] == 0x53) {
        SensorData sensorData;
        sensorData.type = SensorData::RFID;
        memcpy(sensorData.data.rfid, buffer, 24);
        xQueueSend(sensorQueue, &sensorData, portMAX_DELAY);
        
        Serial.println("RFID tag detected");
        tone(PIEZO_PIN, 330, 300);
        delay(100);
        noTone(PIEZO_PIN);
    }
}

No code cleanup. There may be unnecessary code.

Answer 1

You task loop in taskMPU() does not block, it may be that it will block on mpu.getFIFOBytes() but that depends on the implementation of MPU6050 outside of your control. Certainly when dmpReady == false it will never block - not a good idea for any task in a real-time scheduler. It will starve any task of lower priority, and on FreeRTOS on ESP32 it is not wise to relying on round-robin scheduling working for tasks of equal priority as described at https://docs.espressif.com/projects/esp-idf/en/v3.3.3/api-guides/freertos-smp.html#round-robin-scheduling.

taskRFIDCommand() avoids the same issue by at least having a task delay, but at the same priority as taskMPU() there no guarantee it will run periodically, or at all.