【类 型】：fix

【原 因】：原来通过command发送指令
【过 程】：

新增对 Guided 模式的处理逻辑

解析 MQTT 消息中的经纬度和高度参数

判断是否需要导航或仅起飞到指定高度

通过 MAVLink 发送 SET_POSITION_TARGET_GLOBAL_INT 指令实现指点飞行

【影 响】：

src/FoodDeliveryBase.cpp

@@ -774,4 +774,45 @@ void FoodCube::setParam(const char *param_id, float value)
 
   // 通过串口发送数据
   SWrite(buf, len, mavlinkSerial);
+}
+
+/**
+ * @brief 向飞控发送导航目标位置（经纬度 + 高度）
+ * @param lat_deg 纬度（单位：度）
+ * @param lon_deg 经度（单位：度）
+ * @param alt_m 相对高度（单位：米，相对于起飞点）
+ */
+void FoodCube::sendSetPositionTargetGlobalInt(double lat_deg, double lon_deg, double alt_m)
+{
+  mavlink_message_t msg;
+  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+  mavlink_set_position_target_global_int_t sp = {};
+  sp.time_boot_ms = millis();
+  sp.target_system = 1;    // 目标系统 ID（可根据需要调整）
+  sp.target_component = 1; // 目标组件 ID（通常为飞控主组件）
+
+  sp.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
+
+  // 设置 type_mask，仅启用位置控制字段，其它字段被忽略
+  sp.type_mask =
+      (1 << 1) |  // 忽略 vx
+      (1 << 2) |  // 忽略 vy
+      (1 << 3) |  // 忽略 vz
+      (1 << 4) |  // 忽略 afx
+      (1 << 5) |  // 忽略 afy
+      (1 << 6) |  // 忽略 afz
+      (1 << 10) | // 忽略 yaw
+      (1 << 11);  // 忽略 yaw_rate
+
+  sp.lat_int = lat_deg * 1e7;
+  sp.lon_int = lon_deg * 1e7;
+  sp.alt = alt_m;
+
+  // 打包消息 MAVLINK_SYSTEM_ID 255  MAV_COMP_ID 0
+  mavlink_msg_set_position_target_global_int_encode(255, 0, &msg, &sp);
+
+  // 转换为字节流并通过串口发送
+  int len = mavlink_msg_to_send_buffer(buf, &msg);
+  SWrite(buf, len, mavlinkSerial);
 }

src/FoodDeliveryBase.h

@@ -90,6 +90,7 @@ public:
   void mav_send_command(mavlink_command_long_t &msg_cmd);
   void sendCommandAck(uint16_t command, uint8_t result);
   void setParam(const char *param_id, float value);
+  void sendSetPositionTargetGlobalInt(double lat_deg, double lon_deg, double alt_m);
 
   /*云台相机控制*/
   void udpSendToCamera(uint8_t *p_command, uint32_t len);

src/commser.cpp

@@ -180,7 +180,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
       lon = obj["lon"].as<double>();
     }
 
-    addLogMessage("lat:" + String(lat) + " lon:" + String(lon) + " alt:" + String(alt)); // 日志
+    addLogMessage("lat:" + String(lat, 8) + " lon:" + String(lon, 8) + " alt:" + String(alt, 8));
 
     // 根据经纬度是否为0选择起飞或导航
     if (lat == 0.0 && lon == 0.0)
@@ -189,7 +189,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
     }
     else
     {
-      fly_to_location(lat, lon, alt);
+      fc.sendSetPositionTargetGlobalInt(lat, lon, alt); // 发送经纬度和高度
     }
   }
   else if (key == "autoMode") // 自动auto模式
@@ -1079,29 +1079,6 @@ void lock_or_unlock(bool islock)
   fc.mav_send_command(msg_cmd);
 }
 
-/**
- * @brief 飞到指定位置（经纬度+高度），使用导航航点命令（WAYPOINT）
- * @param lat 纬度（单位：度）
- * @param lon 经度（单位：度）
- * @param alt 高度（单位：米）
- */
-void fly_to_location(double lat, double lon, double alt)
-{
-  mavlink_command_long_t msg_cmd = {};
-  msg_cmd.command = MAV_CMD_NAV_WAYPOINT;
-  msg_cmd.param1 = 0;   // 等待时间，单位秒，0表示不停留直接飞
-  msg_cmd.param2 = 0;   // 接收半径，单位米，0表示默认值
-  msg_cmd.param3 = 0;   // 保留参数
-  msg_cmd.param4 = 0;   // 航向角，0表示不指定
-  msg_cmd.param5 = lat; // 目标纬度
-  msg_cmd.param6 = lon; // 目标经度
-  msg_cmd.param7 = alt; // 目标高度
-  msg_cmd.target_system = 1;
-  msg_cmd.target_component = 1;
-  msg_cmd.confirmation = 0;
-  fc.mav_send_command(msg_cmd);
-}
-
 /**
  * @brief 起飞到指定高度，使用起飞命令（TAKEOFF）
  *        一般用于地面起飞，alt为起飞高度