esp32 测距 滤波 代码

当使用ESP32进行距离测量时，可以使用一些滤波算法来平滑测量结果。以下是一个简单的示例代码，演示了如何使用中值滤波对测距结果进行平滑处理：

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_VL53L0X.h>

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

#define NUM_READINGS 5
uint16_t readings[NUM_READINGS];
uint8_t currentIndex = 0;

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

  if (!lox.begin())
  {
    Serial.println(F("Failed to boot VL53L0X"));
    while (1);
  }

  // 初始化读数数组
  for (uint8_t i = 0; i < NUM_READINGS; i++)
  {
    readings[i] = 0;
  }
}

void loop()
{
  // 进行测距
  uint16_t distance = lox.readRangeSingleMillimeters();

  // 更新读数数组
  readings[currentIndex] = distance;
  currentIndex++;

  // 检查是否达到数组末尾
  if (currentIndex >= NUM_READINGS)
  {
    currentIndex = 0;
  }

  // 计算中值
  uint16_t median = calculateMedian(readings, NUM_READINGS);

  // 输出结果
  Serial.print(F("Raw Distance: "));
  Serial.print(distance);
  Serial.print(F(" mm\tFiltered Distance: "));
  Serial.print(median);
  Serial.println(F(" mm"));

  delay(100);
}

// 计算中值
uint16_t calculateMedian(uint16_t* values, uint8_t length)
{
  // 复制数组
  uint16_t sortedValues[length];
  memcpy(sortedValues, values, length * sizeof(uint16_t));

  // 对数组进行排序
  for (uint8_t i = 0; i < length - 1; i++)
  {
    for (uint8_t j = 0; j < length - 1 - i; j++)
    {
      if (sortedValues[j] > sortedValues[j + 1])
      {
        uint16_t temp = sortedValues[j];
        sortedValues[j] = sortedValues[j + 1];
        sortedValues[j + 1] = temp;
      }
    }
  }

  // 返回中值
  return sortedValues[length / 2];
}

这段代码使用Adafruit_VL53L0X库来进行测距，然后使用中值滤波算法对测量结果进行平滑处理。你可以根据自己的需求调整滤波算法和传感器库。希望这能帮到你！如果还有其他问题，请随时提问。