头戴式超声波三维定位跟随无人机系统-【2】

头戴式超声波三维定位跟随无人机系统-【2】
完整内容，包含：

• 反向头戴式超声定位方案
• 时间同步机制（微秒级）
• 三维定位架构
• 6面避障
• 硬件接线
• 完整代码框架

下面是可编译、可直接烧录、注释最详细的完整版代码。

一、无人机端代码（主机：定位 + 避障 + 控制）

文件名：Drone_Main.ino

/* 头戴式超声波TDOA三维定位跟随无人机 模块：无人机主控 ESP32 功能：WiFi同步基准时间 + 3路超声波接收 + 6面避障 + 飞控跟随输出 版本：V1.0 */#include<Arduino.h>#include<WiFi.h>// ==================== WiFi 配置 ====================#defineWIFI_SSID"DRONE_SYNC_2026"#defineWIFI_PASS"12345678"WiFiServerserver(8888);WiFiClient client;// ==================== 超声波接收引脚 ====================#defineR1_PIN2// 左接收器#defineR2_PIN3// 右接收器#defineR3_PIN4// 下接收器// ==================== 6面红外避障 ====================#defineOBST_FRONT12#defineOBST_REAR13#defineOBST_LEFT14#defineOBST_RIGHT15#defineOBST_UP16#defineOBST_DOWN17// ==================== 飞控控制输出 ====================#definePITCH_PIN25// 前后#defineROLL_PIN26// 左右#defineTHROTTLE_PIN27// 上下// ==================== 系统参数 ====================#defineTARGET_DIST80// 目标跟随距离 (cm)#defineTARGET_HEIGHT50// 目标高度差 (cm)#defineSOUND_SPEED0.034// cm/us// ==================== 时间同步变量 ====================volatileuint32_tT0=0;// 基准时间volatileuint32_tt1=0,t2=0,t3=0;volatilebool r1_ok=false,r2_ok=false,r3_ok=false;// ==================== 中断服务程序（必须 IRAM_ATTR）====================voidIRAM_A