在嵌入式系统中同时使用两个传感器时，我想防止性能损失

Question

目前我在lolin d32 pro（esp32）开发板上同时使用mpu6050和uhf rfid模块。

MPU6050的采样率为100hz。我希望RFID也有至少10hz的采样率。

目前，我正在使用 freeRTOS 进行两个单独的测试。根据优先级，

每个方面的性能下降都很严重。

例如，如果IMU传感器的优先级较高，则RFID的识别范围比典型的减少10倍以上。

另外，如果我提高RFID的优先级，IMU数据采集就不顺畅。

两者都获得好的表现很难吗？有没有办法最大化它？

如果您能让我知道，我将不胜感激。

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

没有代码清理。可能有不必要的代码。

Answer 1

taskMPU()

中的任务循环不会阻塞，它可能会在

mpu.getFIFOBytes()

上阻塞，但这取决于

MPU6050

的实现，超出了你的控制范围。 当然，当

dmpReady == false

时它永远不会阻塞——对于实时调度程序中的任何任务来说都不是一个好主意。 它会使任何优先级较低的任务挨饿，并且在 ESP32 上的 FreeRTOS 上，依靠循环调度来处理同等优先级的任务是不明智的，如 https://docs.espressif.com/projects/esp-idf 中所述/en/v3.3.3/api-guides/freertos-smp.html#round-robin-scheduling.

taskRFIDCommand()

至少通过任务延迟来避免相同的问题，但与

taskMPU()

具有相同的优先级，不能保证它会定期运行，或根本不运行。