diff --git a/src/commser.cpp b/src/commser.cpp index 3a89935..bd69e1c 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -358,12 +358,15 @@ void mavlink_receiveCallback(uint8_t c) { mavlink_message_t msg; mavlink_status_t status; - - // 尝试从数据流里 解析数据 + // printf("mav:%d\n",c); //太频繁，导致电机控制处理不过来会乱 + // 尝试从数据流里 解析数据 if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 // 通过msgid来判断 数据流的类别 char buf[10]; + + // printf("mav_id:%d\n",msg.msgid); + switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 @@ -374,11 +377,23 @@ void mavlink_receiveCallback(uint8_t c) topicPubMsg[0] = buf; // 心跳主题 // 从心跳里判断 飞机是否解锁 解锁改变飞机状态 if (heartbeat.base_mode & 128) - { // 从心跳里面 判断已经解锁 + { // 从心跳里面 判断已经解锁 + if (!curr_armed) + { + printf("armed\n"); + fc.playText("[v1]已解锁"); + } + curr_armed = true; topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 } else - { // 心跳里面 判断没有解锁 + { + if (curr_armed) + { + printf("disarm\n"); + fc.playText("[v1]已加锁"); + } + curr_armed = false; // 心跳里面 判断没有解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 if ((uint8_t)topicPubMsg[10].toInt() & 4) { // 如果已经写入了航点 @@ -390,53 +405,59 @@ void mavlink_receiveCallback(uint8_t c) } } // 从心跳里面 判断飞机当前的模式 - switch (heartbeat.custom_mode) + if (curr_mode != heartbeat.custom_mode) { - case 0: - { - topicPubMsg[12] = "Stabilize自稳"; - } - break; - case 2: - { - topicPubMsg[12] = "AltHold定高"; - } - break; - case 3: - { - topicPubMsg[12] = "Auto自动"; - } - break; - case 4: - { - topicPubMsg[12] = "Guided引导"; - } - break; - case 5: - { - topicPubMsg[12] = "Loiter留待"; - } - break; - case 6: - { - topicPubMsg[12] = "RTL返航"; - } - break; - case 9: - { - topicPubMsg[12] = "Land降落"; - } - break; - case 16: - { - topicPubMsg[12] = "PosHold定点"; - } - break; - default: - { - topicPubMsg[12] = "Unknown未知"; - } - break; + curr_mode = heartbeat.custom_mode; + + switch (heartbeat.custom_mode) + { + case 0: + { + topicPubMsg[12] = "姿态"; + } + break; + case 2: + { + topicPubMsg[12] = "定高"; + } + break; + case 3: + { + topicPubMsg[12] = "自动"; + } + break; + case 4: + { + topicPubMsg[12] = "指点"; + } + break; + case 5: + { + topicPubMsg[12] = "悬停"; + } + break; + case 6: + { + topicPubMsg[12] = "返航"; + } + break; + case 9: + { + topicPubMsg[12] = "降落"; + } + break; + case 16: + { + topicPubMsg[12] = "定点"; + } + break; + default: + { + topicPubMsg[12] = "未知模式"; + } + break; + } + fc.playText(topicPubMsg[12]); } } break; @@ -486,15 +507,17 @@ void mavlink_receiveCallback(uint8_t c) mavlink_msg_global_position_int_decode(&msg, &global_position_int); // 高度 sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000); + if (topicPubMsg[4] != buf) { topicPubMsg[4] = buf; } - //{经度，维度，海拔高度} + // 海拔高度 sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}", global_position_int.lon, global_position_int.lat, (double)global_position_int.alt / 1000); + if (topicPubMsg[15] != buf) { topicPubMsg[15] = buf; @@ -584,9 +607,9 @@ void mavlink_receiveCallback(uint8_t c) mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 // 日志 - fc.logln("MsgID:"); + fc.logln((char *)"MsgID:"); fc.logln(msg.msgid); - fc.logln("No:"); + fc.logln((char *)"No:"); fc.logln(mission_request.seq); // 飞控 反馈当前要写入航点的序号 fc.writeSeq = mission_request.seq; @@ -598,9 +621,9 @@ void mavlink_receiveCallback(uint8_t c) mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象 mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据 /*日志*/ - fc.logln("MsgID:"); + fc.logln((char *)"MsgID:"); fc.logln(msg.msgid); - fc.logln("re:"); + fc.logln((char *)"re:"); fc.logln(mission_ark.type); /*记录航点的写入状态 */ fc.missionArkType = mission_ark.type; // 0：写入成功 非0表示失败 @@ -611,6 +634,57 @@ void mavlink_receiveCallback(uint8_t c) } break; + case MAVLINK_MSG_ID_COMMAND_LONG: // #47: 航点提交成果反馈 + { + 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); + 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); + // 没有激光高度直接退出 + if (rngalt_cm == 0) + { + printf("exit rngalt_cm==0"); + break; + } + if (hss == HS_Locked) + { + Hook_autodown(rngalt_cm); + // 语音播报3次 + if (fc.questVoiceStr != "") + fc.playText(fc.questVoiceStr + "。" + fc.questVoiceStr + "。" + fc.questVoiceStr); + } + else + { + if (hss == HS_Stop) + Hook_resume(); + else + printf("exit hooktatus error"); + } + break; + } + // 暂停 + case MAV_CMD_FC_HOOK_PAUSE: + { + printf("MAV_CMD_FC_HOOK_PAUSE \n"); + Hook_stop(); + break; + } + // 暂停 + case MAV_CMD_FC_HOOK_RECOVERY: + { + printf("MAV_CMD_FC_HOOK_RECOVERY \n"); + Hook_recovery(); + break; + } + } + } + break; default: break; }