【类 型】:feat

【原  因】:自动模式切换 校准重量  终端日志打印
【过  程】:
【影  响】:

# 类型 包含:
# feat:新功能(feature)
# fix:修补bug
# docs:文档(documentation)
# style: 格式(不影响代码运行的变动)
# refactor:重构(即不是新增功能,也不是修改bug的代码变动)
# test:增加测试
# chore:构建过程或辅助工具的变动
This commit is contained in:
air 2025-05-09 23:28:31 +08:00
parent 049a5db8e5
commit 408c52bf37
2 changed files with 16 additions and 7 deletions

View File

@ -117,7 +117,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
}
if (topicPubMsg[12] != "自动")
{
fc.playText("自动模式切换失败,请检查飞控是否正常工作");
topicPubMsg[18] = "自动模式切换失败,请检查飞控是否正常工作"; // 终端 打印日志
}
}
}
@ -605,6 +605,7 @@ void mavlink_receiveCallback(uint8_t c)
break;
}
fc.playText(topicPubMsg[12]);
topicPubMsg[18] = "飞控模式切换为" + topicPubMsg[12]; // 终端 打印日志
}
}
break;
@ -814,23 +815,27 @@ void mavlink_receiveCallback(uint8_t c)
{
uint16_t fc_hook_cmd = mavlink_msg_command_long_get_command(&msg);
uint16_t rngalt_cm = (uint16_t)mavlink_msg_command_long_get_param1(&msg);
printf("COMMAND_LONG ID:%d,rng:%dcm \n", fc_hook_cmd, rngalt_cm);
// printf("COMMAND_LONG ID:%d,rng:%dcm \n", fc_hook_cmd, rngalt_cm);
topicPubMsg[18] = "COMMAND_LONG ID:" + String(fc_hook_cmd) + ",rng:" + String(rngalt_cm) + "cm"; // 终端 打印日志
switch (fc_hook_cmd)
{
// 自动放线
case MAV_CMD_FC_HOOK_AUTODOWN:
{
HookStatus hss = motocontrol.gethooktatus();
printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n", hss, rngalt_cm);
// printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n", hss, rngalt_cm);
topicPubMsg[18] = "MAV_CMD_FC_HOOK_AUTODOWN Hook:" + String(hss) + ",rng:" + String(rngalt_cm) + "cm"; // 终端 打印日志
// 没有激光高度直接退出
if (rngalt_cm == 0)
{
printf("exit rngalt_cm==0");
// printf("exit rngalt_cm==0");
topicPubMsg[18] = "exit rngalt_cm==0激光高度异常";
break;
}
if (hss == HS_Locked)
{
Hook_autodown(rngalt_cm);
topicPubMsg[18] = "Hook_autodown";
// 语音播报3次
if (fc.questVoiceStr != "")
fc.playText(fc.questVoiceStr + "" + fc.questVoiceStr + "" + fc.questVoiceStr);
@ -840,21 +845,24 @@ void mavlink_receiveCallback(uint8_t c)
if (hss == HS_Stop)
Hook_resume();
else
printf("exit hooktatus error");
// printf("exit hooktatus error");
topicPubMsg[18] = "exit hooktatus error";
}
break;
}
// 暂停
case MAV_CMD_FC_HOOK_PAUSE:
{
printf("MAV_CMD_FC_HOOK_PAUSE \n");
// printf("MAV_CMD_FC_HOOK_PAUSE \n");
topicPubMsg[18] = "MAV_CMD_FC_HOOK_PAUSE";
Hook_stop();
break;
}
// 收线 收钩
case MAV_CMD_FC_HOOK_RECOVERY:
{
printf("MAV_CMD_FC_HOOK_RECOVERY \n");
// printf("MAV_CMD_FC_HOOK_RECOVERY \n");
topicPubMsg[18] = "MAV_CMD_FC_HOOK_PAUSE";
Hook_recovery();
break;
}

View File

@ -230,6 +230,7 @@ void showinfo()
void begin_tare()
{
ESP_LOGD(MOUDLENAME, "begin_tare");
topicPubMsg[18] = "begin_tare,校准称重";
_weightalign_status = WAS_Aligning;
_needweightalign = true;
_weightalign_begintm = millis();