/// MQTT以及Mavlink通讯控制

#include "commser.h"
#include "motocontrol.h"

extern void Hook_stop();
extern void up_action(float motospeed);
extern Motocontrol motocontrol;
extern void Hook_resume();
extern void begin_tare();
extern bool curr_armed;
extern uint8_t curr_mode;
extern Initstatus initstatus;
extern void Hook_autodown(float length_cm);
extern void Hook_recovery();
static const char *MOUDLENAME = "COMMSER";

/*项目对象*/
// char *ssid = "szMate40pro";    // wifi帐号
// char *password = "63587839ab"; // wifi密码
//   char* ssid = "szdot";                                                             //wifi帐号
//   char* password = "63587839ab";                                                   //wifi密码
char *ssid = "flicube";      // wifi帐号
char *password = "fxmf0622"; // wifi密码
// char *ssid = "fxmf_sc02";           // 4g wifi帐号
// char *password = "12345678";        // 4g wifi密码
char *mqttServer = "szdot.top";     // mqtt地址
int mqttPort = 1883;                // mqtt端口
char *mqttName = "admin";           // mqtt帐号
char *mqttPassword = "123456";      // mqtt密码
uint8_t mavlinkSerial = 2;          // 飞控占用的串口号
uint8_t voiceSerial = 1;            // 声音模块占用串口
char *udpServerIP = "192.168.3.92"; // 云台相机ip
uint32_t udpServerPort = 37260;     // 云台相机端口

FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */
// 登记 json成员名字

//                         0:心跳信息   1:电压信息       2:电流信息        3:电池电量          4:海拔高度     5:对地速度     6:卫星数量  7:定位状态 8:磁罗盘校准进度 9：磁罗盘校准结果 10:任务状态 11:加速度计状态 12:飞机模式 13:重量     14:钩子状态   15:{经，维，高} 16:电池总容量 17:返航点     18:文本信息   19：返航速度  20:{参数名：值}
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "fixType", "completionPct", "reportCal", "questState", "acceState", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed", "parameterValue"};
/*
 *其中topicPubMsg[10]既飞机任务状态的值
 *二进制00 0000
 *第0位： 初始状态           &1
 *第1位： 正在写入航线       &2
 *第2位： 已经写入航点       &4
 *第3位： 正在解锁           &8
 *第4位： 解锁成功           &16
 *第5位： 执行航点任务       &32
 */
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
String topicPubMsg[topicPubCount] = {""};                         // 登记 json成员的值 对应topicPub
/*日志队列*/
#define LOG_QUEUE_SIZE 50 // 日志队列大小
String logQueue[LOG_QUEUE_SIZE];
int logHead = 0;
int logTail = 0;
/*触发发送 主题*/
// 0:对频信息
String topicHandle[] = {"crosFrequency"};

/*异步线程对象*/
Ticker pubTicker; // 定时发布主题 线程
Ticker mavTicker; // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
Ticker funTicker; // 延迟执行
// Ticker chanTicker;   //定时向飞控 发送油门指定
portMUX_TYPE espMutex = portMUX_INITIALIZER_UNLOCKED;
/**
 * @description: mqtt订阅主题 收到信息 的回调函数
 * @param {char*} topic 主题名称 msg/macadd
 * @param {byte*} topic 订阅获取的内容
 * @param {unsigned int} length 内容的长度
 */
void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
{
  /*解构mqtt发过来的内容*/
  String jsonStr = "";
  for (int i = 0; i < length; i++)
  {
    jsonStr += (char)payload[i];
  }
  /*解构内容*/
  DynamicJsonDocument doc(0x2FFF); // 创建 JSON 文档
  deserializeJson(doc, jsonStr);   // 解析 JSON 数据
  // 遍历 JSON 对象
  String key;        // 键
  JsonVariant value; // 值
  for (JsonPair kv : doc.as<JsonObject>())
  {
    key = kv.key().c_str(); // 获取键
    value = kv.value();     // 获取值 取值时 还需指定类型
  }
  /*根据订阅内容 键值 做具体操作*/
  if (key == "questAss")
  {                      // 飞行航点任务 questAss
    String todo = value; // 转换值
    writeRoute(todo);    // 写入航点
  }
  else if (key == "playText")
  {
    JsonObject obj = value.as<JsonObject>(); // 获取嵌套的对象

    String todo = obj["val"] | ""; // 获取文本内容，默认空字符串
    int volInt = obj["vol"] | 1;   // 获取音量，默认1

    // 转为枚举，确保在 1~9 范围内
    volInt = constrain(volInt, 1, 9);
    FoodCube::VoiceVolume volume = (FoodCube::VoiceVolume)volInt;

    fc.playText(todo, volume); // 播放声音
  }
  else if (key == "questAss")
  {                      // 飞行航点任务 questAss
    String todo = value; // 转换值
    writeRoute(todo);    // 写入航点
  }
  else if (key == "setQuestState")
  {
    // 设置飞机任务状态
    String todoJson = value; // 转换值
    /* json 反序列化 */
    DynamicJsonDocument doc(50);
    deserializeJson(doc, todoJson);
    JsonObject obj = doc.as<JsonObject>();
    uint8_t n = obj["bit"];       // 状态位数
    uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
    // 标记飞机状态
    topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
    if (n = 5 && state == 1) // 执行任务 ps:切自动
    {
      funTicker.once(3, []()
                     {
        // 切模式
      set_mode(AUTO); // 设置飞控为自动模式
      funTicker.once(3, []()
                    {
          // 油门
          uint16_t chan[] = {1500, 1500, 1500, 1500};
          fc.mav_channels_override(chan); // 控制油门
        }); });
    }
  }
  else if (key == "unlock") // 解锁
  {
    // 油门
    uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
    fc.mav_channels_override(chan);             // 控制油门
    set_mode(LOITER);                           // 设置飞控为定点模式
    // 可以根据需要调整延迟时间，确保模式切换生效
    funTicker.once(2, []()
                   {
                     lock_or_unlock(false); // 解锁
                   });
  }
  else if (key == "lock") // 加锁
  {
    // 油门
    uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
    fc.mav_channels_override(chan);             // 控制油门
    // 加锁
    lock_or_unlock(true);
    // 延迟切模式
    funTicker.once(2, []()
                   {
                     set_mode(LOITER); // 设置飞控为定点模式
                   });
  }
  else if (key == "guidedMode") // 指点
  {
    // 设置飞控为 Guided 模式
    set_mode(GUIDED);

    // JSON 反序列化 取经纬高
    String todoJson = value;
    DynamicJsonDocument doc(512);
    deserializeJson(doc, todoJson);
    JsonObject obj = doc.as<JsonObject>();

    double alt = obj["alt"].as<double>(); // 高度值
    double lat = 0;
    double lon = 0;

    if (obj.containsKey("lat") && obj.containsKey("lon"))
    {
      lat = obj["lat"].as<double>();
      lon = obj["lon"].as<double>();
    }

    // 根据经纬度是否为0选择起飞或导航
    if (lat == 0.0 && lon == 0.0)
    {
      fly_to_altitude(alt);
    }
    else
    {
      fc.sendSetPositionTargetGlobalInt(lat, lon, alt); // 发送纬经度和高度
    }
  }
  else if (key == "autoMode") // 自动auto模式
  {

    funTicker.once(3, []()
                   {
      // 切模式
      set_mode(AUTO); // 设置飞控为自动模式
      funTicker.once(3,[]()
      {
        // 油门
        uint16_t chan[] = {1500, 1500, 1500, 1500};
        fc.mav_channels_override(chan); // 控制油门
      }); });
  }
  else if (key == "loiterMode") // 悬停定点loiter模式
  {

    // 油门
    uint16_t chan[] = {1500, 1500, 1500, 1500};
    fc.mav_channels_override(chan); // 控制油门
    funTicker.once(0.5, []()
                   {
      // 切模式
      set_mode(LOITER); // 设置飞控为定点模式
      funTicker.once(0.5, []()
                    {
          // 油门
          uint16_t chan[] = {1500, 1500, 1500, 1500};
          fc.mav_channels_override(chan); // 控制油门
        }); });
  }
  else if (key == "holdAltMode") // 定高
  {
    // 油门
    uint16_t chan[] = {1500, 1500, 1500, 1500};
    fc.mav_channels_override(chan); // 控制油门
    funTicker.once(0.5, []()
                   {
      // 切模式
      set_mode(ALT_HOLD); // 设置飞控为定高模式
      funTicker.once(0.5, []()
                    {
          // 油门
          uint16_t chan[] = {1500, 1500, 1500, 1500};
          fc.mav_channels_override(chan); // 控制油门
        }); });
  }
  else if (key == "landMode") // 降落
  {
    set_mode(LAND); // 切模式
  }
  else if (key == "rtlMode") // 返航
  {
    set_mode(RTL); // 切模式
  }
  else if (key == "resetCompass") // 校准磁罗盘
  {
    resetCompass();
  }
  else if (key == "initAcce")
  {
    uint32_t todo = value.as<uint32_t>(); // 转换值
    if (todo == 1)
    {
      initAcce(); // 发起校准
    }
    else
    {
      fc.sendCommandAck(1, 1); // 摆好校准
    }
  }
  else if (key == "setParam")
  {
    String todoJson = value; // 转换值
    /* json 反序列化 */
    DynamicJsonDocument doc(128);
    deserializeJson(doc, todoJson);
    JsonObject obj = doc.as<JsonObject>();
    // 提取参数
    const char *item = obj["item"];  // 获取 item 字段
    float paramValue = obj["value"]; // 获取 value 字段
    // 调用 setParam 函数
    fc.setParam(item, paramValue);
  }
  else if (key == "getParam")
  {
    String todo = value;                 // 转换值
    fc.mav_parameter_data(todo.c_str()); // 获取指定参数 值
  }
  else if (key == "refreshRequest")
  {
    refreshRequest(); // 刷新各种请求
  }
  else if (key == "resetQuestState")
  {                         // 恢复飞机为初始状态
    String todo = value;    // 转换值
    topicPubMsg[10] = todo; // 恢复初始状态
  }
  else if (key == "hookConteroller")
  { // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
    uint16_t todo = value;
    switch (todo)
    {
    case 0:
    {
      // 收钩
      // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_UP");
      addLogMessage("Mqtt_HOOK_UP");
      if (motocontrol.getcontrolstatus().is_autogoodsdown)
        up_action(SPEED_BTN_FAST);
    }
    break;
    case 1:
    {
      // 放钩
    }
    break;
    case 2:
    {
      // 暂停
      // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE");
      addLogMessage("Mqtt_HOOK_PAUSE");
      if (motocontrol.getcontrolstatus().is_autogoodsdown)
        Hook_stop();
    }
    break;
    case 3:
    {
      // 继续
      // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_resume");
      addLogMessage("Mqtt_HOOK_resume");
      if (motocontrol.getcontrolstatus().is_autogoodsdown)
        Hook_resume();
    }
    break;
    case 4:
    {
      // 重置重量
      if (motocontrol.gethooktatus() == HS_Stop) // 先判断钩子状态 “已停止”
        begin_tare();
      else
        fc.playText("未能校准，请保持挂钩，空钩并且没有入仓的状态。再尝试");
    }
    break;
    }
  }
  else if (key == "cameraController")
  {                          // 云台相机控制
    String todoJson = value; // 转换值
    /* json 反序列化 */
    DynamicJsonDocument doc(500);
    deserializeJson(doc, todoJson);
    JsonObject obj = doc.as<JsonObject>();
    int8_t item = obj["item"];
    int8_t val = obj["val"];
    int8_t pitch = obj["pitch"];
    int8_t yaw = obj["yaw"];
    // 相机控制
    if (item == 0)
    {                                                                                       // 0:一键回中
      uint8_t stopCommand[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; // 回中前 强制 俯仰旋转 停止
      size_t stopLen = sizeof(stopCommand);
      fc.udpSendToCamera(stopCommand, stopLen);
      uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
      size_t len = sizeof(command);
      fc.udpSendToCamera(command, len);
    }
    else if (item == 1)
    { // 1:变焦
      uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
      command[8] = val;
      size_t len = sizeof(command);
      fc.udpSendToCamera(command, len);
    }
    else if (item == 2)
    { // 2:俯仰 旋转
      uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
      command[8] = yaw;
      command[9] = pitch;
      size_t len = sizeof(command);
      fc.udpSendToCamera(command, len);
    }
  }
}
/**
 * @description: 写入航点
 * @param {String} todo mqtt订阅执行任务的Json字符串
 */
void writeRoute(String todoJson)
{
  if (fc.writeState) // 如果正在写入状态 跳出
  {
    addLogMessage("misson_writing"); // 提示正在写入中
    return;
  }

  // json 反序列化
  DynamicJsonDocument doc(0x2FFF);
  deserializeJson(doc, todoJson);
  JsonObject obj = doc.as<JsonObject>();
  // 写入航点
  uint8_t taskcount = obj["taskcount"]; // 获取航点总数
  fc.mav_mission_count(taskcount);      // 向飞控请求写入航点的数量
  fc.writeState = true;                 // 锁定写入状态

  topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
  topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点
  // 监听飞控航点写入情况
  while (true)
  {
    fc.comm_receive(mavlink_receiveCallback);
    if (fc.missionArkType == 0)
    {                         // 写入成功
      fc.missionArkType = -1; // “航点写入是否成功”  改成-1默认状态
      addLogMessage("misson_bingo...");
      // 改变飞机状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 航点写入成功状态
      break;
    }
    else if (fc.missionArkType > 0)
    {                         // 写入失败
      fc.missionArkType = -1; // “航点写入是否成功”  改成-1默认状态
      addLogMessage("misson_error...");
      // 改变飞机状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); // 航点写入失败状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 1); // 回到初始状态
      // 当有成果反馈之后 初始化下列数据
      return; // 写入失败 中断函数
    }
    // 飞控返回 新的写入航点序号
    if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1)
    {
      // 从订阅信息里拿航点参数
      uint8_t frame = obj["tasks"][fc.writeSeq]["frame"];
      uint8_t command = obj["tasks"][fc.writeSeq]["command"];
      uint8_t current = obj["tasks"][fc.writeSeq]["current"];
      uint8_t autocontinue = obj["tasks"][fc.writeSeq]["autocontinue"];
      double param1 = obj["tasks"][fc.writeSeq]["param1"];
      double param2 = obj["tasks"][fc.writeSeq]["param2"];
      double param3 = obj["tasks"][fc.writeSeq]["param3"];
      double param4 = obj["tasks"][fc.writeSeq]["param4"];
      double x = obj["tasks"][fc.writeSeq]["x"];
      double y = obj["tasks"][fc.writeSeq]["y"];
      double z = obj["tasks"][fc.writeSeq]["z"];
      String str = obj["tasks"][fc.writeSeq]["sound"];
      if (str.length() > 0 && str != "null")
      {
        fc.questVoiceStr = str;
      }
      addLogMessage("misson_frame--" + String(fc.futureSeq));
      // 写入航点
      fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
      fc.futureSeq++; // 下一个航点序号
    }
  }
}

/**
 * @description: 从串口拿到mavlink缓存数据之后 解析数据执行的回调
 * @param {mavlink_message_t*} pMsg mavlink数据信息指针
 * @param {mavlink_status_t*} pStatus
 * @param {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解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
    // 通过msgid来判断 数据流的类别
    char buf[50];

    // printf("mav_id:%d\n",msg.msgid);
    switch (msg.msgid)
    {
    // case MAVLINK_MSG_ID_RC_CHANNELS: // #65 遥控通道数据
    // {
    //   Serial.println(msg.msgid);
    //   mavlink_rc_channels_t rc_channels;
    //   mavlink_msg_rc_channels_decode(&msg, &rc_channels);

    //   uint16_t throttle_value = rc_channels.chan3_raw; // ch3_raw 是油门通道的原始输
    //   if (topicPubMsg[18] != "油门：" + String(throttle_value))//注意日常添加队列方式改变
    //   {
    //     topicPubMsg[18] = "油门：" + String(throttle_value); // 油门值
    //   }
    // }
    // 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);
      addLogMessage("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);
        addLogMessage("MAV_CMD_FC_HOOK_AUTODOWN Hook:" + String(hss) + ",rng:" + String(rngalt_cm) + "cm"); // 终端 打印日志

        // 没有激光高度直接退出
        if (rngalt_cm == 0)
        {
          // rngalt_cm == 500;
          //  printf("exit rngalt_cm==0");
          addLogMessage("exit rngalt_cm==0，激光高度异常");
          // break;
        }
        if (hss == HS_Locked)
        {
          Hook_autodown(rngalt_cm);
          addLogMessage("Hook_autodown");
          // 语音播报3次
          if (fc.questVoiceStr != "")
          {
            fc.playText(fc.questVoiceStr + "。" + fc.questVoiceStr + "。" + fc.questVoiceStr, fc.V9);
          }
        }
        else
        {
          if (hss == HS_Stop)
            Hook_resume();
          else
            // printf("exit hooktatus error");
            addLogMessage("exit hooktatus error");
        }
        break;
      }
      // 暂停
      case MAV_CMD_FC_HOOK_PAUSE:
      {
        // printf("MAV_CMD_FC_HOOK_PAUSE \n");
        addLogMessage("MAV_CMD_FC_HOOK_PAUSE");
        Hook_stop();
        break;
      }
      // 收线 收钩
      case MAV_CMD_FC_HOOK_RECOVERY:
      {
        // printf("MAV_CMD_FC_HOOK_RECOVERY \n");
        addLogMessage("MAV_CMD_FC_HOOK_PAUSE");
        Hook_recovery();
        break;
      }
      }
    }
    break;
    case MAVLINK_MSG_ID_HOME_POSITION: // #242 返航点位置
    {
      char buf[120];
      mavlink_home_position_t home_pos;
      mavlink_msg_home_position_decode(&msg, &home_pos);

      // 返航点经纬高
      sprintf(buf, "{\"lon\":%d,\"lat\":%d,\"alt\":%.2f}",
              home_pos.longitude, // 经度，以degE7单位传递
              home_pos.latitude,  // 纬度，以degE7单位传递
              home_pos.z);        // 相对高度，单位为米

      topicPubMsg[17] = buf;
    }
    case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: // #49 飞机原点
    {
      mavlink_gps_global_origin_t gps_org;                  // 解构的数据放到这个对象
      mavlink_msg_gps_global_origin_decode(&msg, &gps_org); // 解构msg数据
      // 原点点经纬高
      sprintf(buf, "{\"lon\":%d,\"lat\":%d,\"alt\":%.2f}",
              gps_org.longitude,
              gps_org.latitude,
              (double)gps_org.altitude / 1000);

      // if (topicPubMsg[x] != buf)
      // {
      //   topicPubMsg[x] = buf;
      // }
    }
    break;
    case MAVLINK_MSG_ID_MAG_CAL_REPORT: // #192 磁罗盘校准结果
    {
      mavlink_mag_cal_report_t report;
      mavlink_msg_mag_cal_report_decode(&msg, &report);

      if (report.cal_status == MAG_CAL_SUCCESS)
      {
        topicPubMsg[9] = "successful"; // 校准成功
      }
      else
      {
        topicPubMsg[9] = "failed"; // 校准失败
      }
    }
    break;
    case MAVLINK_MSG_ID_MAG_CAL_PROGRESS: // #191 磁罗盘校准进度
    {
      mavlink_mag_cal_progress_t progress;
      mavlink_msg_mag_cal_progress_decode(&msg, &progress);

      sprintf(buf, "%d", progress.completion_pct);
      topicPubMsg[8] = buf;
    }
    break;
    case MAVLINK_MSG_ID_COMMAND_ACK: // #77 处理 COMMAND_ACK 消息
    {
      mavlink_command_ack_t ack;
      mavlink_msg_command_ack_decode(&msg, &ack);

      // 飞控接收到 校准加速度计命令
      if (ack.command == MAV_CMD_PREFLIGHT_CALIBRATION)
      {
        if (ack.result == 0)
        {
          fc.isAnalyzeAcce = true; // 标记进入 校准状态  此时匹配日志
        }
      }
    }
    break;
    case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
    {
      mavlink_heartbeat_t heartbeat;                  // 解构的数据放到这个对象
      mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据
      sprintf(buf, "%d", heartbeat.base_mode);        // 飞控心跳状态
      topicPubMsg[0] = buf;                           // 心跳主题
      // 从心跳里判断 飞机是否解锁  解锁改变飞机状态
      if (heartbeat.base_mode & 128)
      { // 从心跳里面 判断已经解锁
        if (!curr_armed)
        {
          printf("armed\n");
          fc.playText("已解锁");
        }
        curr_armed = true;
        topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
        topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 0); // 正在解锁状态也关闭
      }
      else
      {
        if (curr_armed)
        {
          printf("disarm\n");
          fc.playText("已加锁");
        }
        curr_armed = false;                                  // 心跳里面 判断没有解锁
        topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为未解锁状态 0xxxx 第四位为0 代表未解锁
        topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 3, 0); // 正在解锁状态也关闭
        if ((uint8_t)topicPubMsg[10].toInt() & 4)
        {                                                      // 如果已经写入了航点
          topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态
        }
        else
        {
          topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态
        }
      }
      // 从心跳里面 判断飞机当前的模式
      if (curr_mode != heartbeat.custom_mode)
      {
        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] = "定点"; // Loiter 定点
        }
        break;
        case 6:
        {
          topicPubMsg[12] = "返航";
        }
        break;
        case 9:
        {
          topicPubMsg[12] = "降落";
        }
        break;
        case 16:
        {
          topicPubMsg[12] = "定点"; // PosHold
        }
        break;
        default:
        {
          topicPubMsg[12] = "未知模式";
        }
        break;
        }
        fc.playText(topicPubMsg[12]);
        addLogMessage("Mode--" + topicPubMsg[12]); // 终端 打印日志
      }
    }
    break;

    case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS
    {
      mavlink_sys_status_t sys_status;                  // 解构的数据放到这个对象
      mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
      // // 电压
      // sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
      // if (topicPubMsg[1] != buf)
      // { // 有更新 则更新数据
      //   topicPubMsg[1] = buf;
      // }
      // 电流
      sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); // 解构中 1=10毫安  所以这里/100 最终单位是A 安培
      if (topicPubMsg[2] != buf)
      {
        topicPubMsg[2] = buf;
      }
      // 电池电量
      sprintf(buf, "%d", sys_status.battery_remaining);
      if (topicPubMsg[3] != buf)
      {
        topicPubMsg[3] = buf;
      }
    }
    break;

    case MAVLINK_MSG_ID_BATTERY_STATUS: // #147: 高压/多电芯电池状态
    {
      mavlink_battery_status_t battery_status;
      mavlink_msg_battery_status_decode(&msg, &battery_status);

      float total_voltage = 0.0f;
      for (int i = 0; i < 10; ++i)
      {
        if (battery_status.voltages[i] != UINT16_MAX)
        {                                                        // 65535表示无效
          total_voltage += battery_status.voltages[i] / 1000.0f; // 转换成 V
        }
      }

      sprintf(buf, "%.2f", total_voltage); // 转成字符串
      if (topicPubMsg[1] != buf)
      {
        topicPubMsg[1] = buf; // 更新电压显示
      }
    }
    break;

    case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU
    {
      mavlink_raw_imu_t raw_imu;
      mavlink_msg_raw_imu_decode(&msg, &raw_imu);
    }
    break;

    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33  位置
    {
      mavlink_global_position_int_t global_position_int;
      mavlink_msg_global_position_int_decode(&msg, &global_position_int);
      // 海拔高度
      sprintf(buf, "%.2f", (double)global_position_int.alt / 1000);

      if (topicPubMsg[4] != buf)
      {
        topicPubMsg[4] = buf;
      }
      // 经纬高
      sprintf(buf, "{\"lon\":%d,\"lat\":%d,\"alt\":%.2f}",
              global_position_int.lon,
              global_position_int.lat,
              (double)global_position_int.relative_alt / 1000);

      if (topicPubMsg[15] != buf)
      {
        topicPubMsg[15] = buf;
      }
    }
    break;

    case MAVLINK_MSG_ID_ADSB_VEHICLE: // #246 ADSB
    {
      mavlink_adsb_vehicle_t adsb;
      mavlink_msg_adsb_vehicle_decode(&msg, &adsb);

      // 提取并转换数据
      uint32_t icao = adsb.ICAO_address;
      float lat = adsb.lat / 1e7;
      float lon = adsb.lon / 1e7;
      int alt = adsb.altitude / 1000;        // 转成米
      float heading = adsb.heading / 100.0f; // 转成度

      // 构建 JSON 字符串
      char jsonString[256];
      snprintf(jsonString, sizeof(jsonString), "{\"icao\":%u,\"lat\":%.7f,\"lon\":%.7f,\"alt\":%d,\"heading\":%.1f}", icao, lat, lon, alt, heading);

      // 发送 MQTT 消息
      fc.pubMQTTmsg("ADSB", jsonString);
    }
    break;

    case MAVLINK_MSG_ID_STATUSTEXT: // #253 文本信息
    {
      char buf[51]; // 定义一个局部缓冲区，大小为 51
      mavlink_statustext_t statustext;
      mavlink_msg_statustext_decode(&msg, &statustext);
      // 解析文本信息
      strncpy(buf, statustext.text, sizeof(statustext.text));
      buf[sizeof(statustext.text)] = '\0'; // 确保字符串以null结尾

      addLogMessage(buf);

      // 解析加速度计校准状态
      if (fc.isAnalyzeAcce)
      {
        // 根据不同的校准状态更新 topicPubMsg[11]，并将结果发送到前端
        if (strstr(buf, "Place vehicle level and press any key") != nullptr)
        {
          topicPubMsg[11] = "level";
        }
        else if (strstr(buf, "Place vehicle on its LEFT side and press any key") != nullptr)
        {
          topicPubMsg[11] = "left";
        }
        else if (strstr(buf, "Place vehicle on its RIGHT side and press any key") != nullptr)
        {
          topicPubMsg[11] = "right";
        }
        else if (strstr(buf, "Place vehicle nose DOWN and press any key") != nullptr)
        {
          topicPubMsg[11] = "down";
        }
        else if (strstr(buf, "Place vehicle nose UP and press any key") != nullptr)
        {
          topicPubMsg[11] = "up";
        }
        else if (strstr(buf, "Place vehicle on its BACK and press any key") != nullptr)
        {
          topicPubMsg[11] = "back";
        }
        else if (strstr(buf, "Calibration successful") != nullptr)
        {
          topicPubMsg[11] = "successful";
          fc.isAnalyzeAcce = false; // 结束校准状态
        }
        else if (strstr(buf, "Calibration FAILED") != nullptr)
        {
          topicPubMsg[11] = "failed";
          fc.isAnalyzeAcce = false; // 结束校准状态
        }
      }
    }
    break;

    case MAVLINK_MSG_ID_VFR_HUD: // #74  hud仪表盘
    {
      mavlink_vfr_hud_t vfr_hud;
      mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
      // 对地速度 ps:飞行速度
      sprintf(buf, "%.2f", vfr_hud.groundspeed);
      if (topicPubMsg[5] != buf)
      {
        topicPubMsg[5] = buf;
      }
    }
    break;

    case MAVLINK_MSG_ID_GPS_RAW_INT: // #24  GPS
    {
      mavlink_gps_raw_int_t gps_raw_int;
      mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
      // 卫星数
      sprintf(buf, "%d", gps_raw_int.satellites_visible);
      if (topicPubMsg[6] != buf)
      {
        topicPubMsg[6] = buf;
      }
      // 卫星状态
      switch (gps_raw_int.fix_type)
      {
      case 0:
        topicPubMsg[7] = "No Fix"; // 无定位
        break;
      case 1:
        topicPubMsg[7] = "Bad Fix"; // 差的定位（无效数据）
        break;
      case 2:
        topicPubMsg[7] = "2D Fix"; // 2D定位（只有经纬度）
        break;
      case 3:
        topicPubMsg[7] = "3D Fix"; // 3D定位（经纬度和高度）
        break;
      case 4:
        topicPubMsg[7] = "DGPS"; // 差分GPS（使用地面站进行修正,不使用rkt最多只能到DGPS）
        break;
      case 5:
        topicPubMsg[7] = "Float"; // RTK浮动解（精度较高，但不如固定解）
        break;
      case 6:
        topicPubMsg[7] = "RTK Fixed"; // RTK固定解（高精度定位）
        break;
      default:
        topicPubMsg[7] = "Unknown"; // 未知状态
        break;
      }
      // 纬度
      // sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
      // 经度
      // sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
    }
    break;

    case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈
    {
      mavlink_mission_request_t mission_request;                  // 解构的数据放到这个对象
      mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
      // 日志
      fc.logln((char *)"MsgID:");
      fc.logln(msg.msgid);
      fc.logln((char *)"No:");
      fc.logln(mission_request.seq);
      // 飞控 反馈当前要写入航点的序号
      fc.writeSeq = mission_request.seq;
    }
    break;

    case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈
    {
      mavlink_mission_ack_t mission_ark;                  // 解构的数据放到这个对象
      mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据
      /*日志*/
      fc.logln((char *)"MsgID:");
      fc.logln(msg.msgid);
      fc.logln((char *)"re:");
      fc.logln(mission_ark.type);
      /*记录航点的写入状态 */
      fc.missionArkType = mission_ark.type; // 0：写入成功 非0表示失败
      /*当有成果反馈之后 初始化下列数据*/
      fc.writeState = false; // 是否是写入状态
      fc.writeSeq = -1;      // 飞控反馈 需写入航点序列号
      fc.futureSeq = 0;      // 记录将来要写入 航点序列号
    }
    break;

    case MAVLINK_MSG_ID_PARAM_VALUE: // #22:获取飞控参数信息
    {
      mavlink_param_value_t param_value;
      mavlink_msg_param_value_decode(&msg, &param_value);

      // "BATT_CAPACITY" 参数 电池总容量
      if (strcmp(param_value.param_id, "BATT_CAPACITY") == 0)
      {
        // 将参数值转换为字符串
        dtostrf(param_value.param_value, 0, 2, buf); // 使用 dtostrf 将浮点数转换为字符串
        if (String(buf) != topicPubMsg[16])
        {
          topicPubMsg[16] = buf;
        }
      }
      // "WPNAV_SPEED" 参数 返航速度
      else if (strcmp(param_value.param_id, "WPNAV_SPEED") == 0)
      {
        // 将参数值转换为字符串
        dtostrf(param_value.param_value / 1000, 0, 2, buf); // 使用 dtostrf 将浮点数转换为字符串
        if (String(buf) != topicPubMsg[19])
        {
          topicPubMsg[19] = buf;
        }
      }
      // 其余参数
      char jsonStr[100];                           // 用于存储 JSON 格式的字符串
      dtostrf(param_value.param_value, 0, 2, buf); // 将浮点数转换为字符串

      // 构造 JSON 格式的字符串 {"param_name": value}
      snprintf(jsonStr, sizeof(jsonStr), "{\"%s\":%.2f}", param_value.param_id, atof(buf));

      // 检查生成的 JSON 字符串是否与 topicPubMsg[20] 不相同，若不同则更新
      if (String(jsonStr) != topicPubMsg[20])
      {
        topicPubMsg[20] = jsonStr;
      }
    }
    break;

    default:
      break;
    }
  }
}

/**
 * @description: 发送主题线程
 */
void pubThread()
{
  /*检测飞控是否返回数据 （此处检测的心跳字段） 没有数据 就像飞控请求*/
  if (topicPubMsg[0] == "") // 检测心跳字段
  {
    refreshRequest();
  }
  /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
  // 创建一个JSON对象
  DynamicJsonDocument doc(512); //  缓冲区
  // 遍历 有更新的数据 组成一个json对象
  for (int i = 0; i < topicPubCount; i++)
  {
    if (i == 0 || i == 10) // 每次必发的写在这里
    {
      doc[topicPub[i]] = topicPubMsg[i];
    }
    else if (i == 8 || i == 9) // 有新值发 但是不重置
    {
      if (topicPubMsg[i] != "")
      {
        doc[topicPub[i]] = topicPubMsg[i];
      }
    }
    else if (i == 18) // 从日志队列里 拿日志信息
    {
      if (logHead != logTail)
      {
        topicPubMsg[i] = logQueue[logHead];
        logHead = (logHead + 1) % LOG_QUEUE_SIZE;
        doc[topicPub[i]] = topicPubMsg[i];
        topicPubMsg[i] = ""; // 清空，避免重复发
      }
    }
    else // 其余有新值就发 发出后重置
    {
      if (topicPubMsg[i] != "")
      {
        doc[topicPub[i]] = topicPubMsg[i];
        topicPubMsg[i] = ""; // 重置
      }
    }
  }
  // 将JSON对象序列化为JSON字符串
  String jsonString;
  serializeJson(doc, jsonString);
  fc.pubMQTTmsg("planeState", jsonString);
}
// 添加日志 到日志队列
void addLogMessage(const String &msg)
{
  portENTER_CRITICAL(&espMutex);

  int nextTail = (logTail + 1) % LOG_QUEUE_SIZE;
  if (nextTail != logHead)
  { // 队列未满
    logQueue[logTail] = msg;
    logTail = nextTail;
  }
  else
  {
    // 队列已满，这里可以选择丢弃或覆盖最旧的日志
    // logHead = (logHead + 1) % LOG_QUEUE_SIZE; // 开启这行表示覆盖最旧的
  }
  portEXIT_CRITICAL(&espMutex);
}

/**
 * @description: 刷新各请求
 */
void refreshRequest()
{
  fc.mav_request_home_position();
  fc.mav_request_data();                  // 请求 设定飞控输出数据流内容
  fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
  fc.mav_parameter_data("WPNAV_SPEED");   // 请求飞控 返回参数 返航速度
}

/**
 * @description: 向飞控 发送油门指令
 */
void chanThread()
{
  // mav_channels_override(channels);
}

/**
 * @brief 设置模式
 * @param 模式
 */
void set_mode(FlightMode mode)
{
  mavlink_command_long_t msg_cmd = {};
  msg_cmd.command = MAV_CMD_DO_SET_MODE;
  msg_cmd.param1 = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
  msg_cmd.param2 = mode;
  msg_cmd.target_system = 1;
  msg_cmd.target_component = 1;
  msg_cmd.confirmation = 0;
  fc.mav_send_command(msg_cmd);
}

/**
 * @brief 加解锁
 * @param islock true加锁 false解锁
 */
void lock_or_unlock(bool islock)
{
  mavlink_command_long_t msg_cmd = {};
  msg_cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
  if (islock)
  {
    msg_cmd.param1 = 0;
    msg_cmd.param2 = 21196;
  }
  else
  {
    msg_cmd.param1 = 1;
    msg_cmd.param2 = 0;
  }
  msg_cmd.target_system = 1;
  msg_cmd.target_component = 1;
  msg_cmd.confirmation = 0;
  fc.mav_send_command(msg_cmd);
}

/**
 * @brief 起飞到指定高度，使用起飞命令（TAKEOFF）
 *        一般用于地面起飞，alt为起飞高度
 * @param alt 高度（单位：米）
 */
void fly_to_altitude(double alt)
{
  mavlink_command_long_t msg_cmd = {};
  msg_cmd.command = MAV_CMD_NAV_TAKEOFF;
  msg_cmd.param1 = 0;   // 起飞时的俯仰角，0表示默认
  msg_cmd.param2 = 0;   // 保留参数
  msg_cmd.param3 = 0;   // 保留参数
  msg_cmd.param4 = 0;   // 保留参数
  msg_cmd.param5 = 0;   // 纬度，起飞时一般无效，设置为0
  msg_cmd.param6 = 0;   // 经度，起飞时一般无效，设置为0
  msg_cmd.param7 = alt; // 起飞目标高度
  msg_cmd.target_system = 1;
  msg_cmd.target_component = 1;
  msg_cmd.confirmation = 0;
  fc.mav_send_command(msg_cmd);
}

/**
 * @brief 校准磁罗盘
 */
void resetCompass()
{
  mavlink_command_long_t msg_cmd = {};
  msg_cmd.command = MAV_CMD_DO_START_MAG_CAL;
  msg_cmd.param1 = 0;
  msg_cmd.param2 = 1;
  msg_cmd.param3 = 0;
  msg_cmd.param4 = 0;
  msg_cmd.param5 = 0;
  msg_cmd.param6 = 0;
  msg_cmd.param7 = 0;
  msg_cmd.target_system = 1;
  msg_cmd.target_component = 1;
  msg_cmd.confirmation = 0;
  fc.mav_send_command(msg_cmd);
}

/**
 * @brief 发起校准
 */
void initAcce()
{
  mavlink_command_long_t msg_cmd = {};
  msg_cmd.command = MAV_CMD_PREFLIGHT_CALIBRATION; // 校准
  msg_cmd.param1 = 0;                              // Gyro Temperature calibration
  msg_cmd.param2 = 0;                              // Magnetometer calibration
  msg_cmd.param3 = 0;                              // Ground Pressure calibration
  msg_cmd.param4 = 0;                              // Remote Control calibration
  msg_cmd.param5 = 1;                              // 1:加速度计
  msg_cmd.param6 = 0;                              // Compass/Motor or Airspeed calibration
  msg_cmd.param7 = 0;                              // ESC or Baro calibration
  msg_cmd.target_system = 1;
  msg_cmd.target_component = 1;
  msg_cmd.confirmation = 0;
  // 发送校准命令
  fc.mav_send_command(msg_cmd);
}