LD2450解析代码

我使用一块LD2450搭配两块ESP32S3，用ESPNOW通信，一端接雷达，一端显示可是始终解析的不对AIGC给的结果也不对.下面的代码是arduino的解析代码，搭配了AIGC去写的
/ ---------- 工具：小端 S16 → 米 / cm ---------- /
static int16_t rawS16(byte lo, byte hi) {
return (int16_t)(lo | (hi << 8));
}

/ 按图例符号位规则 → 米（或 cm） /
static double parseCoord(byte lo, byte hi) {
int16_t raw = rawS16(lo, hi);
if (raw & 0x8000) { // 最高位 1 → 正坐标

return (raw - 32768) / 1000.0;  // 正：raw - 2^15 → m

} else { // 最高位 0 → 负坐标

return -raw / 1000.0;           // 负：0 - raw → m

}
}

/ ---------- 收到 38 字节 ---------- /
void onRecv(const uint8_t mac, const uint8_t data, int len) {
if (len != 38) return;

double x1 = parseCoord(data[4], data[5]); // T1 X 米
double y1 = parseCoord(data[6], data[7]); // T1 Y 米
double v1 = parseCoord(data[8], data[9]) * 10; // T1 速度 cm/s

double x2 = parseCoord(data[16], data[17]); // T2 X 米
double y2 = parseCoord(data[18], data[19]); // T2 Y 米
double v2 = parseCoord(data[20], data[21]) * 10; // T2 速度 cm/s

double x3 = parseCoord(data[28], data[29]); // T3 X 米
double y3 = parseCoord(data[30], data[31]); // T3 Y 米
double v3 = parseCoord(data[32], data[33]) * 10; // T3 速度 cm/s

/ 串口：原始 16 进制 + 解析后物理量 /
Serial.printf("原始 X:0x%02X%02X Y:0x%02X%02X → X1=%+6.2f m Y1=%+6.2f m V1=%+6.2f cm/s X2=%+6.2f m Y2=%+6.2f m V2=%+6.2f cm/s X3=%+6.2f m Y3=%+6.2f m V3=%+6.2f cm/sn",

            data[5], data[4], data[7], data[6], x1, y1, v1,
            data[17], data[16], data[19], data[18], x2, y2, v2,
            data[29], data[28], data[31], data[30], x3, y3, v3);

}

查看更多