From 3045d138b1fc0c66a4c17f149eefb36597ece4ec Mon Sep 17 00:00:00 2001 From: air <30444667+sszdot@users.noreply.github.com> Date: Wed, 21 May 2025 15:45:12 +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=9Afix=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=E5=89=8D=E7=AB=AF=E7=9A=84=E2=80=9C=E7=82=B9=E9=A3=9E?= =?UTF-8?q?=E2=80=9D=E6=8C=87=E4=BB=A4=E4=B8=8E=E8=B5=B7=E9=A3=9E=E6=8C=87?= =?UTF-8?q?=E4=BB=A4=E5=9C=A8=E5=90=8E=E7=AB=AF=E5=9D=87=E4=BD=BF=E7=94=A8?= =?UTF-8?q?=20TAKEOFF=EF=BC=8C=E5=AF=BC=E8=87=B4=E8=A1=8C=E4=B8=BA?= =?UTF-8?q?=E4=B8=8D=E4=B8=80=E8=87=B4=20=E3=80=90=E6=94=B9=20=20=E5=8A=A8?= =?UTF-8?q?=E3=80=91=EF=BC=9A=E5=B0=86=E2=80=9C=E7=82=B9=E9=A3=9E=E2=80=9D?= =?UTF-8?q?=E5=91=BD=E4=BB=A4=E9=80=BB=E8=BE=91=E7=94=B1=20TAKEOFF=20?= =?UTF-8?q?=E6=94=B9=E4=B8=BA=E4=BD=BF=E7=94=A8=20WAYPOINT=EF=BC=8C?= =?UTF-8?q?=E4=BE=9D=E6=8D=AE=E6=98=AF=E5=90=A6=E5=8C=85=E5=90=AB=E6=9C=89?= =?UTF-8?q?=E6=95=88=E7=BB=8F=E7=BA=AC=E5=BA=A6=E5=86=B3=E5=AE=9A=E5=85=B7?= =?UTF-8?q?=E4=BD=93=E9=A3=9E=E8=A1=8C=E6=8C=87=E4=BB=A4=20=E3=80=90?= =?UTF-8?q?=E8=BF=87=20=20=E7=A8=8B=E3=80=91=EF=BC=9A=20-=20=E4=BF=9D?= =?UTF-8?q?=E7=95=99=E5=8E=9F=E6=9C=89=E8=B5=B7=E9=A3=9E=E9=80=BB=E8=BE=91?= =?UTF-8?q?=EF=BC=8C=E5=BD=93=E6=9C=AA=E4=BC=A0=E5=85=A5=E6=9C=89=E6=95=88?= =?UTF-8?q?=E7=BB=8F=E7=BA=AC=E5=BA=A6=EF=BC=880,0=EF=BC=89=E6=97=B6?= =?UTF-8?q?=E9=BB=98=E8=AE=A4=E4=BD=BF=E7=94=A8=20TAKEOFF=20=E8=B5=B7?= =?UTF-8?q?=E9=A3=9E=E8=87=B3=E6=8C=87=E5=AE=9A=E9=AB=98=E5=BA=A6=20-=20?= =?UTF-8?q?=E5=BD=93=E4=BC=A0=E5=85=A5=E6=9C=89=E6=95=88=E7=BB=8F=E7=BA=AC?= =?UTF-8?q?=E5=BA=A6=E6=97=B6=EF=BC=8C=E4=BD=BF=E7=94=A8=20WAYPOINT=20?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=AF=BC=E8=88=AA=E9=A3=9E=E8=A1=8C=20-=20?= =?UTF-8?q?=E6=8B=86=E5=88=86=20fly=5Fto=5Flocation()=20=E4=B8=BA=20fly=5F?= =?UTF-8?q?to=5Flocation()=20=E4=B8=8E=20fly=5Fto=5Faltitude()=EF=BC=8C?= =?UTF-8?q?=E9=80=BB=E8=BE=91=E6=9B=B4=E6=B8=85=E6=99=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 【影 响】： - 实现点飞功能的正确性和可扩展性 - 避免重复起飞逻辑与误导性行为，提高系统稳定性和行为预期一致性 --- src/commser.cpp | 84 ++++++++++++++++++++++++++++++++++--------------- src/commser.h | 5 +-- 2 files changed, 62 insertions(+), 27 deletions(-) diff --git a/src/commser.cpp b/src/commser.cpp index f0832d1..0f6dc49 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -1,5 +1,4 @@ -///MQTT以及Mavlink通讯控制 - +/// MQTT以及Mavlink通讯控制 #include "commser.h" #include "motocontrol.h" @@ -162,23 +161,36 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) { // 设置飞控为 Guided 模式 set_mode(GUIDED); - // 可以根据需要调整延迟时间，确保模式切换生效 + // 确保模式切换生效 delay(2000); + // JSON 反序列化 取经纬高 - String todoJson = value; // 转换值 - DynamicJsonDocument doc(512); // 适当增加缓冲区大小 + String todoJson = value; + DynamicJsonDocument doc(512); deserializeJson(doc, todoJson); JsonObject obj = doc.as(); - double alt = obj["alt"].as(); // 传过来的高度值 + + double alt = obj["alt"].as(); // 高度值 double lat = 0; double lon = 0; - if (obj.containsKey("lat") && obj.containsKey("lon")) // 检查是否存在经纬度 + + if (obj.containsKey("lat") && obj.containsKey("lon")) { lat = obj["lat"].as(); lon = obj["lon"].as(); } - // 飞至指定位置 - fly_to_localtion(lat, lon, alt); + + addLogMessage("lat:" + String(lat) + " lon:" + String(lon) + " alt:" + String(alt)); // 日志 + + // 根据经纬度是否为0选择起飞或导航 + if (lat == 0.0 && lon == 0.0) + { + fly_to_altitude(alt); + } + else + { + fly_to_location(lat, lon, alt); + } } else if (key == "autoMode") // 自动auto模式 { @@ -480,8 +492,8 @@ void mavlink_receiveCallback(uint8_t c) // 没有激光高度直接退出 if (rngalt_cm == 0) { - //rngalt_cm == 500; - // printf("exit rngalt_cm==0"); + // rngalt_cm == 500; + // printf("exit rngalt_cm==0"); addLogMessage("exit rngalt_cm==0，激光高度异常"); // break; } @@ -639,7 +651,7 @@ void mavlink_receiveCallback(uint8_t c) { case 0: { - topicPubMsg[12] = "稳定"; + topicPubMsg[12] = "自稳"; } break; case 2: @@ -988,7 +1000,7 @@ void pubThread() // 添加日志 到日志队列 void addLogMessage(const String &msg) { - portENTER_CRITICAL(&espMutex); + portENTER_CRITICAL(&espMutex); int nextTail = (logTail + 1) % LOG_QUEUE_SIZE; if (nextTail != logHead) @@ -1068,22 +1080,44 @@ void lock_or_unlock(bool islock) } /** - * @brief 飞到指定位置 - * @param lat 维度 - * @param lon 经度 - * @param alt 高度 + * @brief 飞到指定位置（经纬度+高度），使用导航航点命令（WAYPOINT） + * @param lat 纬度（单位：度） + * @param lon 经度（单位：度） + * @param alt 高度（单位：米） */ -void fly_to_localtion(double lat, double lon, double 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为起飞高度 + * @param alt 高度（单位：米） + */ +void fly_to_altitude(double alt) { mavlink_command_long_t msg_cmd = {}; msg_cmd.command = MAV_CMD_NAV_TAKEOFF; - msg_cmd.param1 = 0; - msg_cmd.param2 = 0; - msg_cmd.param3 = 0; - msg_cmd.param4 = 0; - msg_cmd.param5 = lat; - msg_cmd.param6 = lon; - msg_cmd.param7 = alt; + msg_cmd.param1 = 0; // 起飞时的俯仰角，0表示默认 + msg_cmd.param2 = 0; // 保留参数 + msg_cmd.param3 = 0; // 保留参数 + msg_cmd.param4 = 0; // 保留参数 + msg_cmd.param5 = 0; // 纬度，起飞时一般无效，设置为0 + msg_cmd.param6 = 0; // 经度，起飞时一般无效，设置为0 + msg_cmd.param7 = alt; // 起飞目标高度 msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; diff --git a/src/commser.h b/src/commser.h index 088c945..906069a 100644 --- a/src/commser.h +++ b/src/commser.h @@ -9,7 +9,7 @@ // 枚举，表示飞行模式 enum FlightMode { - STABILIZE = 0, // 稳定模式 + STABILIZE = 0, // 自稳模式 ACRO = 1, // 特技模式 ALT_HOLD = 2, // 高度保持模式 AUTO = 3, // 自动模式 @@ -29,7 +29,8 @@ extern void resetCompass(); extern void initAcce(); extern void initAccea(); extern void lock_or_unlock(bool islock); -extern void fly_to_localtion(double lat, double lon, double alt); +extern void fly_to_location(double lat, double lon, double alt); +extern void fly_to_altitude(double alt); extern void writeRoute(String topicStr); extern void mavlink_receiveCallback(uint8_t c); extern const String topicPub[];