From 798be020556f58014ec6611716816667df034bf2 Mon Sep 17 00:00:00 2001
From: tk <szdot@live.com>
Date: Fri, 30 Aug 2024 21:11:37 +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=E5=86=99=E5=85=A5=E5=8F=82=E6=95=B0=20?=
 =?UTF-8?q?=E5=8A=9F=E8=83=BD=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?=
 =?UTF-8?q?=EF=BC=9A=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B=E3=80=91=EF=BC=9A?=
 =?UTF-8?q?=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D=E3=80=91=EF=BC=9A?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/FoodDeliveryBase.cpp | 27 +++++++++++++++++++++++++++
 src/FoodDeliveryBase.h   |  1 +
 src/commser.cpp          | 13 +++++++++++++
 3 files changed, 41 insertions(+)

diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp
index 6b874b4..a949663 100644
--- a/src/FoodDeliveryBase.cpp
+++ b/src/FoodDeliveryBase.cpp
@@ -781,3 +781,30 @@ void FoodCube::sendCommandAck(uint16_t command, uint8_t result)
   // 通过串口发送数据
   SWrite(buf, len, mavlinkSerial);
 }
+
+/**
+ * @brief 向飞控写入参数
+ * @param param_id 参数名称（例如 "COMPASS_OFS_X"）
+ * @param value 参数值（浮点数）
+ */
+void FoodCube::setParam(const char *param_id, float value)
+{
+  mavlink_message_t msg;
+  mavlink_param_set_t param_set;
+
+  // 设置参数
+  param_set.param_value = value;
+  strncpy(param_set.param_id, param_id, sizeof(param_set.param_id));
+  param_set.param_id[sizeof(param_set.param_id) - 1] = '\0'; // 确保字符串以null结尾
+  param_set.target_system = 1;                               // 目标系统 ID
+  param_set.target_component = 1;                            // 目标组件 ID
+
+  // 打包参数设置消息
+  mavlink_msg_param_set_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &param_set);
+
+  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+  int len = mavlink_msg_to_send_buffer(buf, &msg);
+
+  // 通过串口发送数据
+  SWrite(buf, len, mavlinkSerial);
+}
\ No newline at end of file
diff --git a/src/FoodDeliveryBase.h b/src/FoodDeliveryBase.h
index a2dd6bb..1a8e7d6 100644
--- a/src/FoodDeliveryBase.h
+++ b/src/FoodDeliveryBase.h
@@ -76,6 +76,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 udpSendToCamera(uint8_t *p_command, uint32_t len);
diff --git a/src/commser.cpp b/src/commser.cpp
index 4b9fc18..c68c2ec 100644
--- a/src/commser.cpp
+++ b/src/commser.cpp
@@ -213,6 +213,19 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
       fc.sendCommandAck(1, 1); // 摆好校准
     }
   }
+  else if (key == "setParam")
+  {
+    String todoJson = value; // 转换值
+    /* json 反序列化 */
+    DynamicJsonDocument doc(128);
+    deserializeJson(doc, todoJson);
+    JsonObject obj = doc.as<JsonObject>();
+    // 提取参数
+    const char *item = obj["item"];  // 获取 item 字段
+    float paramValue = obj["value"]; // 获取 value 字段
+    // 调用 setParam 函数
+    fc.setParam(item, paramValue);
+  }
   else if (key == "refreshRequest")
   {
     refreshRequest(); // 刷新各种请求