diff --git a/src/commser.cpp b/src/commser.cpp index a792cab..0e26b44 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -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; } diff --git a/src/main.cpp b/src/main.cpp index 3dc6712..a61942c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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();