【类 型】：fix

【主	题】：融合时遗漏代码
【描	述】：
	[原因]：mavlink回调里面 在融合得时候 没有case 飞控控制航点 然而也没有执行
	[过程]：
	[影响]：
【结	束】

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动

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 心跳
@@ -375,10 +378,22 @@ void mavlink_receiveCallback(uint8_t c)
       // 从心跳里判断 飞机是否解锁  解锁改变飞机状态
       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,54 +405,60 @@ void mavlink_receiveCallback(uint8_t c)
         }
       }
       // 从心跳里面 判断飞机当前的模式
+      if (curr_mode != heartbeat.custom_mode)
+      {
+        curr_mode = heartbeat.custom_mode;
+
         switch (heartbeat.custom_mode)
         {
         case 0:
         {
-        topicPubMsg[12] = "Stabilize自稳";
+          topicPubMsg[12] = "姿态";
         }
         break;
         case 2:
         {
-        topicPubMsg[12] = "AltHold定高";
+          topicPubMsg[12] = "定高";
         }
         break;
         case 3:
         {
-        topicPubMsg[12] = "Auto自动";
+          topicPubMsg[12] = "自动";
         }
         break;
         case 4:
         {
-        topicPubMsg[12] = "Guided引导";
+          topicPubMsg[12] = "指点";
         }
         break;
         case 5:
         {
-        topicPubMsg[12] = "Loiter留待";
+          topicPubMsg[12] = "悬停";
         }
         break;
         case 6:
         {
-        topicPubMsg[12] = "RTL返航";
+          topicPubMsg[12] = "返航";
         }
         break;
         case 9:
         {
-        topicPubMsg[12] = "Land降落";
+          topicPubMsg[12] = "降落";
         }
         break;
         case 16:
         {
-        topicPubMsg[12] = "PosHold定点";
+          topicPubMsg[12] = "定点";
         }
         break;
         default:
         {
-        topicPubMsg[12] = "Unknown未知";
+          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;
     }