From d713db3c1dd16eea59e0a2ee606cc2ee9c496838 Mon Sep 17 00:00:00 2001
From: tk <szdot@live.com>
Date: Thu, 15 Aug 2024 13:56:53 +0800
Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?=
 =?UTF-8?q?=EF=BC=9Afeat=20=E9=A3=9E=E6=8E=A7=E5=8F=96WPNAV=5FSPEED=20?=
 =?UTF-8?q?=E8=BF=94=E8=88=AA=E9=80=9F=E5=BA=A6=E5=8F=82=E6=95=B0=20?=
 =?UTF-8?q?=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91=EF=BC=9A=E5=89=8D?=
 =?UTF-8?q?=E7=AB=AF=E7=94=A8=E6=9D=A5=E8=AE=A1=E7=AE=97=20=E8=BF=94?=
 =?UTF-8?q?=E8=88=AA=E7=94=B5=E9=87=8F=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B?=
 =?UTF-8?q?=E3=80=91=EF=BC=9A=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D=E3=80=91?=
 =?UTF-8?q?=EF=BC=9A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/commser.cpp | 24 ++++++++++++++++++------
 1 file changed, 18 insertions(+), 6 deletions(-)

diff --git a/src/commser.cpp b/src/commser.cpp
index 1c72a6f..507527f 100644
--- a/src/commser.cpp
+++ b/src/commser.cpp
@@ -34,8 +34,8 @@ uint32_t udpServerPort = 37260;     // 云台相机端口
 FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象
 /* 发布 主题 ps:以下是登记发布json内容的组成元素 */
 // 登记 json成员名字
-// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:海拔高度 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，相对高度} 16:电池总容量 17:返航点 18:文本信息
-const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText"};
+// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:海拔高度 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，相对高度} 16:电池总容量 17:返航点 18:文本信息 19：返航速度
+const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"};
 const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
 String topicPubMsg[topicPubCount] = {""};                         // 登记 json成员的值 对应topicPub
 /*触发发送 主题*/
@@ -709,7 +709,7 @@ void mavlink_receiveCallback(uint8_t c)
       mavlink_param_value_t param_value;
       mavlink_msg_param_value_decode(&msg, &param_value);
 
-      // 只处理 "BATT_CAPACITY" 参数
+      // "BATT_CAPACITY" 参数 电池总容量
       if (strcmp(param_value.param_id, "BATT_CAPACITY") == 0)
       {
         // 将参数值转换为字符串
@@ -719,6 +719,16 @@ void mavlink_receiveCallback(uint8_t c)
           topicPubMsg[16] = buf;
         }
       }
+      // "WPNAV_SPEED" 参数 返航速度
+      if (strcmp(param_value.param_id, "WPNAV_SPEED") == 0)
+      {
+        // 将参数值转换为字符串
+        dtostrf(param_value.param_value / 1000, 0, 2, buf); // 使用 dtostrf 将浮点数转换为字符串
+        if (String(buf) != topicPubMsg[19])
+        {
+          topicPubMsg[19] = buf;
+        }
+      }
     }
     break;
 
@@ -772,11 +782,13 @@ void pubThread()
 void refreshRequest()
 {
   fc.mav_request_home_position();
-  delay(100);
+  delay(50);
   fc.mav_request_data(); // 请求 设定飞控输出数据流内容
-  delay(100);
+  delay(50);
   fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
-  delay(100);
+  delay(50);
+  fc.mav_parameter_data("WPNAV_SPEED"); // 请求飞控 返回参数 返航速度
+  delay(50);
 }
 
 /**