From 65623b01bcd218d96763f20a3f55c008ffcf66b4 Mon Sep 17 00:00:00 2001 From: air <30444667+sszdot@users.noreply.github.com> Date: Thu, 22 May 2025 20:18:52 +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=9Atask1=E6=A0=B8=E5=BF=83=20=E8=BF=90=E8=A1=8C=E5=8D=A1?= =?UTF-8?q?=E9=A1=BF=20delay=E5=A4=AA=E5=A4=9A=20=E3=80=90=E8=BF=87=20=20?= =?UTF-8?q?=E7=A8=8B=E3=80=91=EF=BC=9Amqtt=E7=9A=84=E5=9B=9E=E8=B0=83?= =?UTF-8?q?=E4=B8=ADdelay=20=E5=85=A8=E9=83=A8=E6=94=B9=E4=B8=BA=E5=BC=82?= =?UTF-8?q?=E6=AD=A5=E6=89=A7=E8=A1=8C=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D?= =?UTF-8?q?=E3=80=91=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动 --- src/commser.cpp | 106 +++++++++++++++++++++++++++--------------------- src/main.cpp | 69 ++++++++++++++----------------- 2 files changed, 90 insertions(+), 85 deletions(-) diff --git a/src/commser.cpp b/src/commser.cpp index df6e4c6..ce9537b 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -63,6 +63,7 @@ String topicHandle[] = {"crosFrequency"}; /*异步线程对象*/ Ticker pubTicker; // 定时发布主题 线程 Ticker mavTicker; // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 +Ticker funTicker; // 延迟执行 // Ticker chanTicker; //定时向飞控 发送油门指定 portMUX_TYPE espMutex = portMUX_INITIALIZER_UNLOCKED; /** @@ -128,13 +129,16 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state); if (n = 5 && state == 1) // 执行任务 ps:切自动 { - delay(3000); - // 切模式 + funTicker.once(3, []() + { + // 切模式 set_mode(AUTO); // 设置飞控为自动模式 - // 油门 - delay(3000); - uint16_t chan[] = {1500, 1500, 1500, 1500}; - fc.mav_channels_override(chan); // 控制油门 + funTicker.once(3, []() + { + // 油门 + uint16_t chan[] = {1500, 1500, 1500, 1500}; + fc.mav_channels_override(chan); // 控制油门 + }); }); } } else if (key == "unlock") // 解锁 @@ -143,9 +147,11 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底 fc.mav_channels_override(chan); // 控制油门 set_mode(LOITER); // 设置飞控为定点模式 - delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效 - lock_or_unlock(false); // 解锁 - // 功能待修改 判断有没有在地面上 然后切定点 确认切定点油门设1100 + // 可以根据需要调整延迟时间，确保模式切换生效 + funTicker.once(2, []() + { + lock_or_unlock(false); // 解锁 + }); } else if (key == "lock") // 加锁 { @@ -154,15 +160,16 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) fc.mav_channels_override(chan); // 控制油门 // 加锁 lock_or_unlock(true); - delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效 - set_mode(LOITER); // 设置飞控为定点模式 + // 延迟切模式 + funTicker.once(2, []() + { + set_mode(LOITER); // 设置飞控为定点模式 + }); } else if (key == "guidedMode") // 指点 { // 设置飞控为 Guided 模式 set_mode(GUIDED); - // 确保模式切换生效 - delay(2000); // JSON 反序列化 取经纬高 String todoJson = value; @@ -180,8 +187,6 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) lon = obj["lon"].as(); } - addLogMessage("lat:" + String(lat, 8) + " lon:" + String(lon, 8) + " alt:" + String(alt, 8)); - // 根据经纬度是否为0选择起飞或导航 if (lat == 0.0 && lon == 0.0) { @@ -194,37 +199,50 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) } else if (key == "autoMode") // 自动auto模式 { - delay(3000); - // 切模式 - set_mode(AUTO); // 设置飞控为自动模式 - // 油门 - delay(3000); - uint16_t chan[] = {1500, 1500, 1500, 1500}; - fc.mav_channels_override(chan); // 控制油门 + + funTicker.once(3, []() + { + // 切模式 + set_mode(AUTO); // 设置飞控为自动模式 + funTicker.once(3,[]() + { + // 油门 + uint16_t chan[] = {1500, 1500, 1500, 1500}; + fc.mav_channels_override(chan); // 控制油门 + }); }); } else if (key == "loiterMode") // 悬停定点loiter模式 { + // 油门 uint16_t chan[] = {1500, 1500, 1500, 1500}; fc.mav_channels_override(chan); // 控制油门 - delay(500); - // 切模式 - set_mode(LOITER); // 设置飞控为定点模式 - // 油门 - delay(500); - fc.mav_channels_override(chan); // 控制油门 + funTicker.once(0.5, []() + { + // 切模式 + set_mode(LOITER); // 设置飞控为定点模式 + funTicker.once(0.5, []() + { + // 油门 + uint16_t chan[] = {1500, 1500, 1500, 1500}; + fc.mav_channels_override(chan); // 控制油门 + }); }); } else if (key == "holdAltMode") // 定高 { // 油门 uint16_t chan[] = {1500, 1500, 1500, 1500}; fc.mav_channels_override(chan); // 控制油门 - delay(500); - // 切模式 - set_mode(ALT_HOLD); - // 油门 - delay(500); - fc.mav_channels_override(chan); // 控制油门 + funTicker.once(0.5, []() + { + // 切模式 + set_mode(ALT_HOLD); // 设置飞控为定高模式 + funTicker.once(0.5, []() + { + // 油门 + uint16_t chan[] = {1500, 1500, 1500, 1500}; + fc.mav_channels_override(chan); // 控制油门 + }); }); } else if (key == "landMode") // 降落 { @@ -371,7 +389,7 @@ void writeRoute(String todoJson) { if (fc.writeState) // 如果正在写入状态 跳出 { - fc.logln("正在写航点"); // 提示正在写入中 + addLogMessage("misson_writing"); // 提示正在写入中 return; } @@ -393,7 +411,7 @@ void writeRoute(String todoJson) if (fc.missionArkType == 0) { // 写入成功 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 - fc.logln("misson_bingo..."); + addLogMessage("misson_bingo..."); // 改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 @@ -403,7 +421,7 @@ void writeRoute(String todoJson) else if (fc.missionArkType > 0) { // 写入失败 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 - fc.logln("misson_error..."); + addLogMessage("misson_error..."); // 改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); // 航点写入失败状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 @@ -431,13 +449,11 @@ void writeRoute(String todoJson) { fc.questVoiceStr = str; } - fc.logln("frame--"); - fc.logln(frame); + addLogMessage("misson_frame--" + String(fc.futureSeq)); // 写入航点 fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); fc.futureSeq++; // 下一个航点序号 } - delay(20); } } @@ -696,7 +712,7 @@ void mavlink_receiveCallback(uint8_t c) break; } fc.playText(topicPubMsg[12]); - addLogMessage("飞控模式已切换为" + topicPubMsg[12]); // 终端 打印日志 + addLogMessage("Mode--" + topicPubMsg[12]); // 终端 打印日志 } } break; @@ -1022,13 +1038,9 @@ void addLogMessage(const String &msg) void refreshRequest() { fc.mav_request_home_position(); - delay(50); - fc.mav_request_data(); // 请求 设定飞控输出数据流内容 - delay(50); + fc.mav_request_data(); // 请求 设定飞控输出数据流内容 fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值 - delay(50); - fc.mav_parameter_data("WPNAV_SPEED"); // 请求飞控 返回参数 返航速度 - delay(50); + fc.mav_parameter_data("WPNAV_SPEED"); // 请求飞控 返回参数 返航速度 } /** diff --git a/src/main.cpp b/src/main.cpp index 4412654..4d5a50a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,5 @@ -///主程序，硬件及线程控制 -/// @file main.cpp +/// 主程序，硬件及线程控制 +/// @file main.cpp #include #include "HX711.h" #include "OneButton.h" @@ -76,12 +76,10 @@ bool _checkweightcal = false; // 检测是否要检测称重传感器是否需 uint8_t _checkweighttimes = 0; // unsigned long _tm_checkweigh; static const char *MOUDLENAME = "MAIN"; -unsigned long _tm_core1 ; //主核心1 循环开始时间 -unsigned long _tm_core0 ; //核心0 循环开始时间 -unsigned long _looptm_core1=0 ; //主核心1单次循环时间--评估是否超时卡顿 -unsigned long _looptm_core0=0 ; //核心0单次循环时间--评估是否超时卡顿 - - +unsigned long _tm_core1; // 主核心1 循环开始时间 +unsigned long _tm_core0; // 核心0 循环开始时间 +unsigned long _looptm_core1 = 0; // 主核心1单次循环时间--评估是否超时卡顿 +unsigned long _looptm_core0 = 0; // 核心0单次循环时间--评估是否超时卡顿 // 称重校准状态 enum Weightalign_status @@ -105,7 +103,7 @@ void setup() // 调试串口 Serial.begin(57600); - ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION); + // ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION); preferences.begin("PullupDev", false); wei_offset = preferences.getLong("wei_offset", 0); // ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset); @@ -138,9 +136,9 @@ void setup() // 初始化RGB LED 显示黄色灯表示需要注意 LED FastLED.addLeds(leds, NUM_LEDS); // GRB ordering is typical if (!motocontrol.init(&myservo)) // 初始化电机控制 - ESP_LOGE(MOUDLENAME, "motocontrol init fault"); + // ESP_LOGE(MOUDLENAME, "motocontrol init fault"); - initstatus = IS_WaitStart; + initstatus = IS_WaitStart; _tm_waitinit = millis(); _needweightalign = (wei_offset == 0); @@ -289,10 +287,10 @@ bool check_tare() // void checkinited() { - //根据初始化状态initstatus操作 + // 根据初始化状态initstatus操作 switch (initstatus) { - //刚开机，等待500ms + // 刚开机，等待500ms case Initstatus::IS_WaitStart: { if ((millis() - _tm_waitinit) > 500) @@ -418,7 +416,7 @@ void set_locked(bool locked) // 在核心1上执行，重要的延迟低的 void loop() { - _tm_core1= millis(); + _tm_core1 = millis(); // sercomm.getcommand(); // 得到控制命令 button_checktop.tick(); // 按钮 button_down.tick(); // 按钮 @@ -453,15 +451,14 @@ void loop() check_tare(); // 检查看是否需要校准称重 checkinited(); // 检测执行初始化工作 delay(1); - _looptm_core1= millis()-_tm_core1; - //如果循环时间超过1000ms,则打印错误日志 - - if (_looptm_core1>10) - { - // ESP_LOGE(MOUDLENAME, "main loop time out %d ms", _looptm_core1); - addLogMessage("core1 timeout: " + String(_looptm_core1)); - } + _looptm_core1 = millis() - _tm_core1; + // 如果循环时间超过1000ms,则打印错误日志 + if (_looptm_core1 > 10) + { + // ESP_LOGE(MOUDLENAME, "main loop time out %d ms", _looptm_core1); + addLogMessage("core1 timeout: " + String(_looptm_core1)); + } } // 在核心0上执行耗时长的低优先级的 void Task1(void *pvParameters) @@ -474,34 +471,29 @@ void Task1(void *pvParameters) // 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了） while (1) { - _tm_core0= millis(); + _tm_core0 = millis(); + /*重量*/ if (_needweightalign) { _needweightalign = false; scale.tare(); } - pullweight = get_pullweight(); - // 显示重量 - // printf("pullweight: %d \n", pullweight); - /////////////////////////////////MQTT_语音_MAVLINK 部分 + pullweight = get_pullweight(); + /*从飞控拿数据*/ fc.comm_receive(mavlink_receiveCallback); - vTaskDelay(1); - /////////////////////////////////MQTT_语音_MAVLINK 部分结束 + // vTaskDelay(1); if (fc.checkWiFiStatus()) - /*保持mqtt心跳,如果Mqtt没有连接会自动连接*/ - fc.mqttLoop("cmd"); + fc.mqttLoop("cmd"); // 保持mqtt心跳,如果Mqtt没有连接会自动连接 vTaskDelay(10); - _looptm_core0= millis()-_tm_core0; - - //如果循环时间超过100ms,则打印错误日志 - if (_looptm_core0>200) + + _looptm_core0 = millis() - _tm_core0; + /*如果循环时间超过100ms,则打印错误日志*/ + if (_looptm_core0 > 50) { - // ESP_LOGE(MOUDLENAME, "main loop time out %d ms", _looptm_core1); addLogMessage("core0 timeout: " + String(_looptm_core0)); } - } } void sendinfo() // 每500ms发送状态信息 @@ -791,7 +783,8 @@ void Hook_autodown(float length_cm) motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 } else - ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked "); + addLogMessage("autodown fault, not HS_Locked"); + // ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked "); } void Hook_stop() {