src/main.cpp

@@ -66,7 +66,6 @@ unsigned long _tm_bengstop;
 bool _bengstop = false;
 bool _needweightalign = false;
 
-
 bool curr_armed = false;
 uint8_t curr_mode = 0;
 // 称重校准状态
@@ -106,9 +105,11 @@ const uint16_t   MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
 /*项目对象*/
 // char* ssid = "szdot";                                                             //wifi帐号
 // char* password = "Ttaj@#*.com";                                                   //wifi密码
-char* ssid = (char*)"fxmf_sc01";          //wifi帐号
-char* password =  (char*)"12345678";     //wifi密码
-char* mqttServer =  (char*)"114.115.137.239";//"szdot.top";  //mqtt地址
+// char* ssid = (char*)"fxmf_sc01";          //wifi帐号
+// char* password =  (char*)"12345678";     //wifi密码
+char *ssid = (char *)"flicube";              // wifi帐号
+char *password = (char *)"fxmf0622";         // wifi密码
+char *mqttServer = (char *)"152.32.162.75";  //"szdot.top";  //mqtt地址
 int mqttPort = 1883;                         // mqtt端口
 char *mqttName = (char *)"admin";            // mqtt帐号
 char *mqttPassword = (char *)"123456";       // mqtt密码
@@ -131,7 +132,24 @@ String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState"
 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]);                                                                                                         // 订阅主题总数
 /*有更新主动发送 主题*/
 // 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度)
-String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus","position",};
+String topicPub[] = {
+    "heartBeat",
+    "voltagBattery",
+    "currentBattery",
+    "batteryRemaining",
+    "positionAlt",
+    "groundSpeed",
+    "satCount",
+    "latitude",
+    "longitude",
+    "fixType",
+    "planeState",
+    "pingNet",
+    "getPlaneMode",
+    "loadweight",
+    "hookstatus",
+    "position",
+};
 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 发送主题总数
 String topicPubMsg[16];                                     // 发送数据存放 对应topicPub
 String oldMsg[16];                                          // 记录旧的数据 用来对比有没有更新
@@ -159,9 +177,6 @@ void setup()
   wei_offset = preferences.getLong("wei_offset", 0);
   printf("wei_offset:%d\n", wei_offset);
 
-
-
-
   // 初始化按钮
   button_up.attachClick(upbtn_click);
   button_up.attachDoubleClick(upbtn_dbclick);
@@ -191,7 +206,6 @@ void setup()
   if (!motocontrol.init(&myservo))                            // 初始化电机控制
     printf("motocontrol init fault\n");
 
-  
   tksendinfo.start();
   initstatus = IS_WaitStart;
   _tm_waitinit = millis();
@@ -215,9 +229,6 @@ void setup()
   // flashTicker.start();  //监听 按flash键时 主动发布对频主题
   /////////////////////////////////MQTT_语音_MAVLINK 部分结束
 
-
-
-
   // if (motocontrol.getstatus()==MS_Stop)
   // {
   //     //slowup();
@@ -255,7 +266,6 @@ void showinfo()
   msg_cmd.confirmation = 0;
   fc.mav_send_command(msg_cmd);
   // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
-
 }
 // 校准称重
 void begin_tare()
@@ -289,7 +299,9 @@ bool check_tare()
       motocontrol.weightalign(true);
       printf("check_tare ok: %d,offset:%d \n", pullweight, wei_offset);
       return true;
-     }else _needweightalign = true;
+    }
+    else
+      _needweightalign = true;
   }
   else
   {
@@ -351,7 +363,8 @@ void checkinited()
       motocontrol.weightalign(true);
       _needweightalign = false;
       initstatus = IS_OK;
-    }else
+    }
+    else
     {
       printf("begin_tare \n");
       begin_tare();
@@ -364,8 +377,8 @@ void checkinited()
     if (_weightalign_status == WAS_AlignOK)
     {
       initstatus = IS_OK;
-    }else
-    if (_weightalign_status==WAS_Alignfault)
+    }
+    else if (_weightalign_status == WAS_Alignfault)
     {
       printf("weightalign fault! again \n");
       initstatus = IS_begin_WeightZero;
@@ -373,7 +386,6 @@ void checkinited()
     break;
   }
 
-
     /*
       case Initstatus::IS_LengthZero:
       {
@@ -423,7 +435,6 @@ void loop()
   pubTicker.update();  // 定时 发布主题
   mavTicker.update();  // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
 
- 
   // sercomm.getcommand();   // 得到控制命令
   button_checktop.tick(); // 按钮
   button_down.tick();     // 按钮
@@ -463,7 +474,6 @@ void loop()
   fc.comm_receive(mavlink_receiveCallback);
   /////////////////////////////////MQTT_语音_MAVLINK 部分结束
   delay(1);
-
 }
 // 在核心0上执行耗时长的低优先级的
 void Task1(void *pvParameters)
@@ -563,8 +573,6 @@ void showledidel()
     led_show(255, 255, 255); // 白色
     break;
   }
-
-
   }
   /*
 
@@ -647,7 +655,8 @@ void down_action(float motospeed,float length = 0.0f)
     if (motocontrol.getcontrolstatus().is_autogoodsdown)
     {
       motocontrol.moto_goodsdownresume();
-    }else
+    }
+    else
     {
       motocontrol.setspeed(motospeed);
       motocontrol.moto_run(MotoStatus::MS_Down, length);
@@ -666,7 +675,8 @@ void up_action(float motospeed)
     motocontrol.setspeed(motospeed);
     motocontrol.moto_run(MotoStatus::MS_Up);
     fc.playText("[v1]收线");
-  }else
+  }
+  else
     motocontrol.stoprun();
 }
 // 放线按钮--单击
@@ -687,7 +697,8 @@ void downbtn_pressstart()
   Serial.println("Down_pressstart");
   // 两个同时按用于对频
   btn_pressatonce++;
-  if (btn_pressatonce>2) btn_pressatonce=2;
+  if (btn_pressatonce > 2)
+    btn_pressatonce = 2;
   if (btn_pressatonce == 2)
   {
     btn_presssatonce();
@@ -700,7 +711,8 @@ void downbtn_pressend()
 {
   Serial.println("Down_pressend");
   btn_pressatonce--;
-  if (btn_pressatonce<0) btn_pressatonce=0;
+  if (btn_pressatonce < 0)
+    btn_pressatonce = 0;
   // 不是恢复自动放线抬起按键就停止
   if (!motocontrol.getcontrolstatus().is_autogoodsdown)
     motocontrol.stoprun();
@@ -733,7 +745,8 @@ void upbtn_pressstart()
   Serial.println("UP_pressstart");
   // 两个同时按用于对频
   btn_pressatonce++;
-  if (btn_pressatonce>2) btn_pressatonce=2;
+  if (btn_pressatonce > 2)
+    btn_pressatonce = 2;
   if (btn_pressatonce == 2)
   {
     btn_presssatonce();
@@ -746,7 +759,8 @@ void upbtn_pressend()
 {
   Serial.println("UP_pressend");
   btn_pressatonce--;
-  if (btn_pressatonce<0) btn_pressatonce=0;
+  if (btn_pressatonce < 0)
+    btn_pressatonce = 0;
 
   motocontrol.stoprun();
 }
@@ -758,7 +772,8 @@ void Hook_autodown(float length_cm)
   {
     printf("Hook_autodown %.2f cm \n", length_cm);
     motocontrol.moto_goodsdown(length_cm); // 二楼340  //桌子40
-  }else
+  }
+  else
     Serial.println("autodown fault, not HS_Locked ");
 }
 void Hook_stop()
@@ -772,7 +787,8 @@ void Hook_resume()
   {
     printf("Hook_resume\n");
     motocontrol.moto_goodsdownresume();
-  }else
+  }
+  else
     Serial.println("resume fault, not HS_Stop ");
 }
 void Hook_recovery()
@@ -820,23 +836,28 @@ void testbtn_click()
  * @param {byte*} topic 订阅获取的内容
  * @param {unsigned int} length 内容的长度
  */
-void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
+void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
+{
   /*获取 "订阅主题/macadd" 截取/macadd后的长度 */
   int count = strlen(topic);
   String cutTopic = ""; // 记录订阅主题
-  for (int i = 0; i < count - 13; i++) {
+  for (int i = 0; i < count - 13; i++)
+  {
     cutTopic += topic[i];
   }
   /*解构mqtt发过来的内容*/
   String topicStr = "";
-  for (int i = 0; i < length; i++) {
+  for (int i = 0; i < length; i++)
+  {
     topicStr += (char)payload[i];
   }
   /*订阅回调主体*/
-  if (cutTopic == topicSub[0]) {  //0:飞行航点任务 questAss
+  if (cutTopic == topicSub[0])
+  {                       // 0:飞行航点任务 questAss
     writeRoute(topicStr); // 写入航点
-  } else
-  if (cutTopic == topicSub[1]) {  //1:设置飞机状态 setPlaneState
+  }
+  else if (cutTopic == topicSub[1])
+  { // 1:设置飞机状态 setPlaneState
     /*
      *其中topicPubMsg[10]既飞机状态的值
      *二进制0000 0000 0000 0000
@@ -862,68 +883,85 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
     uint8_t count = obj["count"]; // 传过来
     // 解构val数组参数
     uint16_t param[count];
-    for (int i = 0; i < count; i++) {
+    for (int i = 0; i < count; i++)
+    {
       param[i] = obj["param"][i];
     }
     // 标记飞机状态
     topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
     // 飞控执行
-    if (n == 3) {                                    //3操作飞机加解锁
+    if (n == 3)
+    {                                             // 3操作飞机加解锁
       uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
       fc.mav_channels_override(chan);             // 控制油门
       fc.mav_set_mode(2);                         // 飞控设置成AltHold定高
       fc.mav_command(n, param);
-    } else {
+    }
+    else
+    {
       uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
       fc.mav_channels_override(chan);             // 控制油门
     }
-    if (n == 6) {                //6测试起飞
+    if (n == 6)
+    {                           // 6测试起飞
       fc.mav_set_mode(4);       // 飞控设置成Guided引导模式
       fc.mav_command(n, param); // 起飞
     }
-    if (n == 7) {          //7 悬停
+    if (n == 7)
+    {                     // 7 悬停
       fc.mav_set_mode(5); // 飞控设置成Loiter留待模式
     }
-    if (n == 5) {          //5 航点执行
+    if (n == 5)
+    {                     // 5 航点执行
       fc.mav_set_mode(3); // 飞控设置成auto自动模式
     }
-    if (n == 8) {  //8降落*
+    if (n == 8)
+    { // 8降落*
       fc.mav_command(n, param);
     }
-    if (n == 9) {          //9返航
+    if (n == 9)
+    {                     // 9返航
       fc.mav_set_mode(6); // 飞控设置成RTL返航
     }
-    if (n == 11) {  //11磁罗盘校准
+    if (n == 11)
+    { // 11磁罗盘校准
       fc.mav_command(n, param);
     }
   }
-  if (cutTopic == topicSub[2]) {                   //2:获取飞机状态 getPlaneState
+  if (cutTopic == topicSub[2])
+  {                                               // 2:获取飞机状态 getPlaneState
     fc.pubMQTTmsg("planeState", topicPubMsg[10]); // 终端主动get飞机状态
-  }else
-  if (cutTopic == topicSub[3]) {  //3:恢复飞机为初始状态 resetState
+  }
+  else if (cutTopic == topicSub[3])
+  {                        // 3:恢复飞机为初始状态 resetState
     topicPubMsg[10] = "1"; // 恢复初始状态
-  }else
-  if (cutTopic == topicSub[4]) {                   //4:油门通道1
+  }
+  else if (cutTopic == topicSub[4])
+  {                                               // 4:油门通道1
     uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
     fc.channels[0] = strInt;                      // 恢复初始状态
     fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[5]) {                   //5:油门通道2
+  }
+  else if (cutTopic == topicSub[5])
+  {                                               // 5:油门通道2
     uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
     fc.channels[1] = strInt;                      // 恢复初始状态
     fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[6]) {                   //6:油门通道3
+  }
+  else if (cutTopic == topicSub[6])
+  {                                               // 6:油门通道3
     uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
     fc.channels[2] = strInt;                      // 恢复初始状态
     fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[7]) {                   //7:油门通道4
+  }
+  else if (cutTopic == topicSub[7])
+  {                                               // 7:油门通道4
     uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
     fc.channels[3] = strInt;                      // 恢复初始状态
     fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[8]) {                   //8:钩子控制
+  }
+  else if (cutTopic == topicSub[8])
+  {                                               // 8:钩子控制
     uint16_t strInt = (uint16_t)topicStr.toInt(); //
     printf("hookcontrol command: %d \n", strInt);
     switch (strInt)
@@ -950,7 +988,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
         Hook_resume();
       break;
     }
-  }else if (cutTopic == topicSub[9]) {            //9:云台相机控制 cameraController
+  }
+  else if (cutTopic == topicSub[9])
+  { // 9:云台相机控制 cameraController
     // json 反序列化
     DynamicJsonDocument doc(0x2FFF);
     deserializeJson(doc, topicStr);
@@ -959,17 +999,21 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
     uint8_t item = obj["item"];
 
     size_t len;
-    if (item ==0) {  //0:一键回中
+    if (item == 0)
+    { // 0:一键回中
       uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
       len = sizeof(command);
       fc.udpSendToCamera(command, len);
-
-    } else if (item == 1) {  //1:变焦
+    }
+    else if (item == 1)
+    { // 1:变焦
       uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
       command[8] = obj["val"];
       len = sizeof(command);
       fc.udpSendToCamera(command, len);
-    } else if (item == 2) {  //2:俯仰,旋转
+    }
+    else if (item == 2)
+    { // 2:俯仰,旋转
       uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
       int8_t pitchvalue = obj["pitch"];
       int8_t yawvalue = obj["yaw"];
@@ -980,7 +1024,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
       fc.udpSendToCamera(command, len);
       // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
     }
-  }else if (cutTopic == topicSub[10]) {            //通用命令
+  }
+  else if (cutTopic == topicSub[10])
+  { // 通用命令
     // json 反序列化
     DynamicJsonDocument doc(0x2FFF);
     deserializeJson(doc, topicStr);
@@ -1002,7 +1048,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
  * @description: 写入航点
  * @param {String} topicStr mqtt订阅执行任务的Json字符串
  */
-void writeRoute(String topicStr) {
+void writeRoute(String topicStr)
+{
   if (fc.writeState) // 如果正在写入状态 跳出
   {
     fc.logln((char *)"正在写航点"); // 提示正在写入中
@@ -1021,10 +1068,12 @@ void writeRoute(String topicStr) {
   fc.mav_mission_count(taskcount);      // 向飞控请求写入航点的数量
   fc.writeState = true;                 // 锁定写入状态
   // 监听飞控航点写入情况
-  while (true) {
+  while (true)
+  {
     topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点
     fc.comm_receive(mavlink_receiveCallback);
-    if (fc.missionArkType == 0) {  //写入成功
+    if (fc.missionArkType == 0)
+    {                         // 写入成功
       fc.missionArkType = -1; // “航点写入是否成功”  改成-1默认状态
       fc.logln((char *)"misson_bingo...");
       // 改变飞机状态
@@ -1032,7 +1081,9 @@ void writeRoute(String topicStr) {
       topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
       topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
       break;
-    } else if (fc.missionArkType > 0) {  //写入失败
+    }
+    else if (fc.missionArkType > 0)
+    {                         // 写入失败
       fc.missionArkType = -1; // “航点写入是否成功”  改成-1默认状态
       fc.logln((char *)"misson_error...");
       // 改变飞机状态
@@ -1042,7 +1093,8 @@ void writeRoute(String topicStr) {
       return; // 写入失败 中断函数
     }
     // 飞控返回 新的写入航点序号
-    if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) {
+    if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1)
+    {
       // 从订阅信息里拿航点参数
       uint8_t frame = obj["tasks"][fc.writeSeq]["frame"];
       uint8_t command = obj["tasks"][fc.writeSeq]["command"];
@@ -1079,18 +1131,21 @@ void writeRoute(String topicStr) {
  * @param {mavlink_status_t*} pStatus
  * @param {uint8_t} c 串口读取的缓存
  */
-void mavlink_receiveCallback(uint8_t c) {
+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解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
+  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) {
+    switch (msg.msgid)
+    {
     case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
     {
       mavlink_heartbeat_t heartbeat;                  // 解构的数据放到这个对象
@@ -1098,7 +1153,8 @@ void mavlink_receiveCallback(uint8_t c) {
       sprintf(buf, "%d", heartbeat.base_mode);        // 飞控心跳状态
       topicPubMsg[0] = buf;                           // 心跳主题
       // 从心跳里判断 飞机是否解锁  解锁改变飞机状态
-          if (heartbeat.base_mode & 128) {                          //从心跳里面 判断已经解锁
+      if (heartbeat.base_mode & 128)
+      { // 从心跳里面 判断已经解锁
         if (!curr_armed)
         {
           printf("armed\n");
@@ -1106,7 +1162,9 @@ void mavlink_receiveCallback(uint8_t c) {
         }
         curr_armed = true;
         topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
-          } else {  
+      }
+      else
+      {
         if (curr_armed)
         {
           printf("disarm\n");
@@ -1114,9 +1172,12 @@ void mavlink_receiveCallback(uint8_t c) {
         }
         curr_armed = false;                                  // 心跳里面 判断没有解锁
         topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
-            if ((uint8_t)topicPubMsg[10].toInt() & 4) {             //如果已经写入了航点
+        if ((uint8_t)topicPubMsg[10].toInt() & 4)
+        {                                                      // 如果已经写入了航点
           topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态
-            } else {
+        }
+        else
+        {
           topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态
         }
       }
@@ -1125,7 +1186,8 @@ void mavlink_receiveCallback(uint8_t c) {
       {
         curr_mode = heartbeat.custom_mode;
 
-          switch (heartbeat.custom_mode) {
+        switch (heartbeat.custom_mode)
+        {
         case 0:
         {
           topicPubMsg[12] = "姿态";
@@ -1183,17 +1245,20 @@ void mavlink_receiveCallback(uint8_t c) {
       mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
       // 电压
       sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
-          if (topicPubMsg[1] != buf) {  // 有更新 则更新数据
+      if (topicPubMsg[1] != buf)
+      { // 有更新 则更新数据
         topicPubMsg[1] = buf;
       }
       // 电流
       sprintf(buf, "%.2f", (double)sys_status.current_battery / 100);
-          if (topicPubMsg[2] != buf) {
+      if (topicPubMsg[2] != buf)
+      {
         topicPubMsg[2] = buf;
       }
       // 电池电量
       sprintf(buf, "%d", sys_status.battery_remaining);
-          if (topicPubMsg[3] != buf) {
+      if (topicPubMsg[3] != buf)
+      {
         topicPubMsg[3] = buf;
       }
     }
@@ -1220,18 +1285,18 @@ void mavlink_receiveCallback(uint8_t c) {
       // 高度
       sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
 
-         
-          if (topicPubMsg[4] != buf) {
+      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
-            );
+              (double)global_position_int.alt / 1000);
 
-          if (topicPubMsg[15] != buf) {
+      if (topicPubMsg[15] != buf)
+      {
         topicPubMsg[15] = buf;
       }
     }
@@ -1243,7 +1308,8 @@ void mavlink_receiveCallback(uint8_t c) {
       mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
       // 对地速度 ps:飞行速度
       sprintf(buf, "%.2f", vfr_hud.groundspeed);
-          if (topicPubMsg[5] != buf) {
+      if (topicPubMsg[5] != buf)
+      {
         topicPubMsg[5] = buf;
       }
     }
@@ -1255,21 +1321,25 @@ void mavlink_receiveCallback(uint8_t c) {
       mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
       // 卫星数
       sprintf(buf, "%d", gps_raw_int.satellites_visible);
-          if (topicPubMsg[6] != buf) {
+      if (topicPubMsg[6] != buf)
+      {
         topicPubMsg[6] = buf;
       }
       // 纬度
       sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
-          if (topicPubMsg[7] != buf) {
+      if (topicPubMsg[7] != buf)
+      {
         topicPubMsg[7] = buf;
       }
       // 经度
       sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
-          if (topicPubMsg[8] != buf) {
+      if (topicPubMsg[8] != buf)
+      {
         topicPubMsg[8] = buf;
       }
       // 卫星状态
-          switch (gps_raw_int.fix_type) {
+      switch (gps_raw_int.fix_type)
+      {
       case 0:
       {
         topicPubMsg[9] = "No Fix";
@@ -1390,7 +1460,6 @@ void mavlink_receiveCallback(uint8_t c) {
         break;
       }
       }
-          
     }
     break;
     default:
@@ -1402,20 +1471,27 @@ void mavlink_receiveCallback(uint8_t c) {
 /**
  * @description: 发送主题线程
  */
-void pubThread() {
+void pubThread()
+{
   /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/
-  for (int i = 0; i < topicPubCount; i++) {  //遍历向订阅主题 有数据更新时 发送信息
-    if (topicPubMsg[i] != oldMsg[i]) {
-      if (i == 0) {  //心跳包 每每向心跳主题发布信息
+  for (int i = 0; i < topicPubCount; i++)
+  { // 遍历向订阅主题 有数据更新时 发送信息
+    if (topicPubMsg[i] != oldMsg[i])
+    {
+      if (i == 0)
+      { // 心跳包 每每向心跳主题发布信息
         // 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
-        if (fc.getIsInit()) {
+        if (fc.getIsInit())
+        {
           fc.setIsInit(false);
           fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容
         }
         // 发送心跳包
         fc.pubMQTTmsg(topicPub[0], topicPubMsg[0]);
         // printf("pub%s:%s\n",topicPub[0],topicPubMsg[0]);
-      } else {  //非心跳包 有更新才向对应主题发布信息
+      }
+      else
+      { // 非心跳包 有更新才向对应主题发布信息
         fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]);
         oldMsg[i] = topicPubMsg[i];
       }
@@ -1428,14 +1504,19 @@ void pubThread() {
 /**
  * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
  */
-void flashThread() {
-  if (digitalRead(23) == HIGH) {
-    if (isPush) {  //点击之后
+void flashThread()
+{
+  if (digitalRead(23) == HIGH)
+  {
+    if (isPush)
+    { // 点击之后
       // 请求注册 ps:发送esp8266的物理地址 到对频主题
       fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
     }
     isPush = false; // 复位按钮
-  } else {
+  }
+  else
+  {
     // FLASH按下状态
     isPush = true;
   }
@@ -1444,13 +1525,15 @@ void flashThread() {
 /**
  * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
  */
-void mavThread() {
+void mavThread()
+{
   fc.mav_request_data();
 }
 
 /**
  * @description: 向飞控 发送油门指令
  */
-void chanThread() {
+void chanThread()
+{
   // mav_channels_override(channels);
 }