【类 型】：fix
【原 因】：task1核心 运行卡顿 delay太多 【过 程】：mqtt的回调中delay 全部改为异步执行 【影 响】： # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
This commit is contained in:
air
parent
562beb6ce9
commit
65623b01bc
106
src/commser.cpp
106
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<double>();
|
||||
}
|
||||
|
||||
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"); // 请求飞控 返回参数 返航速度
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
69
src/main.cpp
69
src/main.cpp
@ -1,5 +1,5 @@
|
||||
///主程序，硬件及线程控制
|
||||
/// @file main.cpp
|
||||
/// 主程序，硬件及线程控制
|
||||
/// @file main.cpp
|
||||
#include <Arduino.h>
|
||||
#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<WS2812, LED_DATA_PIN, RGB>(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()
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user