#include "FoodDeliveryBase.h"
/// MQTT和Mavlink业务逻辑控制
///  @file FoodDeliveryBase.cpp
/**
 * @description: 初始化
 */
static const char *MOUDLENAME = "FC";
FoodCube::FoodCube(char *userSsid, char *userPassword, char *userMqttServer, int userMqttPort, char *userMqttName, char *userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char *userUdpServerIP, uint32_t userUdpServerPort)
{
  /*初始化*/
  ssid = userSsid;                   // wifi帐号
  password = userPassword;           // wifi密码
  mqttServer = userMqttServer;       // mqtt服务器地址
  mqttPort = userMqttPort;           // mqtt服务器端口
  mqttName = userMqttName;           // mqtt帐号
  mqttPassword = userMqttPassword;   // mqtt密码
  mavlinkSerial = userMavlinkSerial; // mavlink用的串口
  voiceSerial = userVoiceSerial;     // 声音模块用的串口
  udpServerIP = userUdpServerIP;     // 云台相机ip
  udpServerPort = userUdpServerPort; // 云台相机端口
  wificonnected = false;
  _tm_mqttconnect = 0;

  // 初始化飞控通讯串口 波特率
  switch (mavlinkSerial)
  { // 初始化指定 串口号
  case 0:
    Serial.begin(57600, SERIAL_8N1);
    // 设置接收缓冲区大小为1024字节
    Serial.setRxBufferSize(1024);
    break;
  case 1:
    Serial1.begin(57600, SERIAL_8N1);
    // 设置接收缓冲区大小为1024字节
    Serial1.setRxBufferSize(1024);
    break;
  case 2:
    Serial2.begin(57600, SERIAL_8N1);
    // 设置接收缓冲区大小为1024字节
    Serial2.setRxBufferSize(2048); // 飞控用
    break;
  }
  // 初始化声音模块串口 波特率
  switch (voiceSerial)
  { // 初始化指定 串口号
  case 0:
    Serial.begin(115200);
    break;
  case 1:
    Serial1.begin(115200);
    break;
  case 2:
    Serial2.begin(115200);
    break;
  }
  /*初始化 连接mqtt对象 赋予mqttClient指针*/
  mqttClient = new PubSubClient(mqttServer, mqttPort, wifiClient);
  SetplayvolMax();
}
/**
 * @description: 日志打印
 * @param {*} val 输出的信息
 */
void FoodCube::log(String val)
{
  Serial.print(val);
}
void FoodCube::log(char *val)
{
  Serial.print(val);
}
void FoodCube::log(int val)
{
  Serial.print(val);
}
void FoodCube::log(IPAddress val)
{
  Serial.print(val);
}
void FoodCube::log(bool val)
{
  Serial.print(val);
}
void FoodCube::logln(String val)
{
  ESP_LOGD(MOUDLENAME, "%s", val);
}
void FoodCube::logln(char *val)
{
  ESP_LOGD(MOUDLENAME, "%s", val);
}
void FoodCube::logln(int val)
{
  ESP_LOGD(MOUDLENAME, "%d", val);
}
void FoodCube::logln(IPAddress val)
{
  ESP_LOGD(MOUDLENAME, "%s", val.toString());
}
void FoodCube::logln(bool val)
{
  Serial.print(val);
}

/**
 *@description: 取值 设置值
 */
String FoodCube::getMacAdd()
{
  return macAdd;
}

/**
 * @description: 连接wifi
 */
// 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解）
IPAddress local_IP(192, 168, 8, 201);
// 设置静态IP网关
IPAddress gateway(192, 168, 8, 1);

IPAddress subnet(255, 255, 255, 0);
IPAddress primaryDNS(8, 8, 8, 8);   // optional
IPAddress secondaryDNS(8, 8, 4, 4); // optional

void FoodCube::connectWifi()
{
  // 设置wifi帐号密码
  WiFi.begin(ssid, password);
  // 连接wifi
  logln("Connecting Wifi...");
}

bool FoodCube::checkWiFiStatus()
{
  if (!wificonnected && (WiFi.status() == WL_CONNECTED))
  {
    wificonnected = true;
    // 获取局域网ip
    logln("");
    logln("WiFi connected");
    log("IP address: ");
    logln(WiFi.localIP());
    localIp = WiFi.localIP();
    // 设置开发板为无线终端 获取物理mac地址
    WiFi.mode(WIFI_STA);
    macAdd = WiFi.macAddress();
    macAdd.replace(":", ""); // 板子的物理地址 并且去除冒号
    log("macAdd: ");
    logln(macAdd);
    playText("网络连接成功");
  }
  return wificonnected;
}
/**
 * @description: 连接mqtt
 * @param {String} topicSub 主题
 */
void FoodCube::connectMqtt(String topicSub)
{
  if (mqttClient->connected())
    return;
  /*尝试连接mqtt*/
  if ((millis() - _tm_mqttconnect) > 3000)
  {
    logln("connect_mqtt");
    if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword))
    {
      logln("MQTT Server Connected.");
      log("Server Address: ");
      logln(mqttServer);
      log("ClientId :");
      logln(macAdd);
      /*连接成功 订阅主题*/
      // 订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback
      subscribeTopic(topicSub, 1);
      delay(500);
      playText("服务器已连接");
    }
    else
    {
      // 失败返回状态码
      log("MQTT Server Connect Failed. Client State:");
      logln(mqttClient->state());
      _tm_mqttconnect = millis();
      playText("服务器断开尝试重新连接");
      // delay(3000);
    }
  }
}

/**
 * @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
 * @param {String} topicSub 主题
 */
void FoodCube::mqttLoop(String topicSub)
{
  if (mqttClient->connected())
  {                     // 检测 如果开发板成功连接服务器
    mqttClient->loop(); // 保持心跳
  }
  else
  {                        // 如果开发板未能成功连接服务器
    connectMqtt(topicSub); // 则尝试连接服务器
  }
}

/**
 * @description: 订阅指定主题
 * @param {String} topicString 主题名称
 * @param {int} Qos 1：重要 ps:响应时间快
 */
void FoodCube::subscribeTopic(String topicString, int Qos = 1)
{
  // 处理主题字符串
  topicString = topicString + "/" + macAdd;
  char subTopic[topicString.length() + 1];
  strcpy(subTopic, topicString.c_str());
  // 订阅主题
  mqttClient->subscribe(subTopic, Qos);
}

/**
 * @description: 向指定主题 发布信息
 * @param {String} topicString 主题名称
 * @param {String} messageString 发布的内容
 */
void FoodCube::pubMQTTmsg(String topicString, String messageString)
{
  // 处理主题字符串
  topicString = topicString + "/" + macAdd;
  char publishTopic[topicString.length() + 1];
  strcpy(publishTopic, topicString.c_str());
  // 处理发布内容字符串
  char publishMsg[messageString.length() + 1];
  strcpy(publishMsg, messageString.c_str());
  // 向指定主题 发布信息
  mqttClient->publish(publishTopic, publishMsg);
}

/**
 * @description: 判断并向指定串口 发送命令帧
 * @param {uint8_t[]} buf[]  命令帧 字节数组
 * @param {int} len 命令帧 的长度
 * @param {uint8_t} swSerial 串口选择
 */
void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial)
{
  // 通过串口发送
  switch (swSerial)
  { // 初始化指定 串口号
  case 0:
    Serial.write(buf, len);
    break;
  case 1:
    Serial1.write(buf, len);
    break;
  case 2:
    Serial2.write(buf, len);
    break;
  }
}

void FoodCube::SetplayvolMax()
{
  // 输出音量开到最大
  uint8_t volcommand[10] = {0xFD, 0x00, 0x07, 0x01, 0x01, 0x5B, 0x76, 0x31, 0x30, 0x5D};
  SWrite(volcommand, sizeof(volcommand), voiceSerial);
}
/**
 * @brief 播放语音文本（可指定音量）
 * @param str 播放的文本内容（UTF-8 编码）
 * @param vol 音量等级（枚举 V1~V9），默认为 V1（最小音量）
 */
void FoodCube::playText(String str, VoiceVolume vol)
{
  unsigned long currentTime = millis();
  // 判断是否到了可以执行函数的时间
  if (currentTime - lastRunTime > 3000)
  {
    lastRunTime = currentTime; // 更新上次执行时间
  }
  else
  {
    return; // 频繁为 达到3秒 不播放声音退出
  }
  // 拼接音量控制前缀，例如 "[v5]起飞"
  String vstr = "[v" + String((int)vol) + "]" + str;

  // 计算消息长度，语音模块要求帧数据 = 内容长度 + 2
  int len = vstr.length();
  if (len >= 3996)
  {
    // 防止超长导致模块错误
    return;
  }

  int frameLength = len + 2;
  byte highByte = (frameLength >> 8) & 0xFF;
  byte lowByte = frameLength & 0xFF;

  // 构造完整命令帧：帧头 + 长度 + 命令 + 编码类型 + 内容
  uint8_t command[len + 5];
  int index = 0;
  command[index++] = 0xFD;     // 帧头
  command[index++] = highByte; // 长度高位
  command[index++] = lowByte;  // 长度低位
  command[index++] = 0x01;     // 命令字：播放合成
  command[index++] = 0x04;     // 编码方式：UTF-8

  // 填充文本内容部分
  for (int i = 0; i < len; i++)
  {
    command[index++] = (uint8_t)vstr[i];
  }

  // 发送数据到语音串口
  SWrite(command, sizeof(command), voiceSerial);

  // 延时等待模块处理，防止连续播放死机
  // delay(300); // 可根据模块处理时间调节
}

/**
 * @description: 检查声音模块是否空闲
 * @return {uint8_t} 0x4E 78:系统忙  0x4F 79:系统空闲
 */
uint8_t FoodCube::chekVoiceMcu()
{
  uint8_t serialData;
  uint8_t check[] = {0xFD, 0x00, 0x01, 0x21}; // 检查命令帧
  SWrite(check, sizeof(check), voiceSerial);  // 发送检查指令
  switch (voiceSerial)
  {
  case 0:
  {
    while (Serial.available())
    {                             // 当串口接收到信息后
      serialData = Serial.read(); // 将接收到的信息使用read读取
    }
  }
  break;
  case 1:
  {
    while (Serial1.available())
    {                              // 当串口接收到信息后
      serialData = Serial1.read(); // 将接收到的信息使用read读取
    }
  }
  break;
  case 2:
  {
    while (Serial2.available())
    {                              // 当串口接收到信息后
      serialData = Serial2.read(); // 将接收到的信息使用read读取
    }
  }
  break;
  }

  return serialData;
}

/**
 * @description: 声音模块停止合成
 */
void FoodCube::stopVoice()
{
  uint8_t stop[] = {0xFD, 0x00, 0x01, 0x22}; // 停止合成命令帧
  SWrite(stop, sizeof(stop), voiceSerial);   // 发送检查指令
}

/**
 * @description: 相机控制命令帧 校检码
 * @param {uint8_t[]} 命令帧 数组
 * @param {int} len 命令帧 的长度
 */
uint16_t FoodCube::CRC16_cal(uint8_t *ptr, uint32_t len, uint16_t crc_init)
{
  uint16_t crc, oldcrc16;
  uint8_t temp;
  crc = crc_init;
  while (len-- != 0)
  {
    temp = (crc >> 8) & 0xff;
    oldcrc16 = crc16_tab[*ptr ^ temp];
    crc = (crc << 8) ^ oldcrc16;
    ptr++;
  }
  return (crc);
}

void FoodCube::crc_check_16bites(uint8_t *pbuf, uint32_t len, uint16_t *p_result)
{
  uint16_t crc_result = 0;
  crc_result = CRC16_cal(pbuf, len, 0);
  *p_result = crc_result;
}

/**
 * @description: 相机 向下
 */
void FoodCube::Camera_action_down()
{
  Camera_action_move(-100, 0); // 俯仰 向下
}
/**
 * @description: 相机 角度控制
 */
void FoodCube::Camera_action_move(uint8_t vpitch, uint8_t vyaw)
{
  uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
  command[8] = vyaw;   // 旋转
  command[9] = vpitch; // 俯仰
  udpSendToCamera(command, sizeof(command));
}

/**
 * @description: 相机 角度控制
 */
void FoodCube::Camera_action_zoom(uint8_t vzoom)
{
  uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
  command[8] = vzoom; // 变焦
  udpSendToCamera(command, sizeof(command));
}

/**
 * @description: 相机 回中
 */
void FoodCube::Camera_action_ret()
{
  uint8_t command1[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
  udpSendToCamera(command1, sizeof(command1));
  uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
  udpSendToCamera(command, sizeof(command));
}

/**
 * @description: 相机
 * @param {uint8_t[]} 命令帧 数组
 * @param {int} len 命令帧 的长度
 */
void FoodCube::udpSendToCamera(uint8_t *p_command, uint32_t len)
{
  uint16_t result = 0;
  uint16_t *p_result = &result;
  // 计算校检码
  crc_check_16bites(p_command, len, p_result);
  // 加上校检码
  uint8_t bytes[2 + len];
  for (int i = 0; i < len; i++)
  {
    bytes[i] = p_command[i];
  }
  bytes[len] = static_cast<uint8_t>(result);          // 低位 互换
  bytes[len + 1] = static_cast<uint8_t>(result >> 8); // 高位 互换

  /*  //发送日志
  printf("SendToCamera:");
  for (int i=0;i<len+2;i++)
  printf("0x%02X,", bytes[i]);
  printf("\n");
  */
  // udp发送命令帧
  udp.beginPacket(udpServerIP, udpServerPort);
  udp.write(bytes, len + 2);
  udp.endPacket();
}

/**
 * @description: 按位设置 1 or 0
 * @param {String} str 要设置的值
 * @param {uint8_t} n 第几位 从0开始
 * @param {uint8_t} isOne 1:设置成1 0:设置成0
 */
String FoodCube::setNBit(String str, uint8_t n, uint8_t i)
{
  char buf[10];
  uint16_t val = (uint8_t)str.toInt();
  if (i)
  {
    val |= (1u << n); // 按位设置成1
  }
  else
  {
    val &= ~(1u << n); // 按位设置成0
  }
  sprintf(buf, "%d", val);
  return buf;
}

/**
 * @description: 向飞控请求 指定数据
 */
void FoodCube::mav_request_data()
{
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  // 以下注释 把数据流组合到一起
  /*
   * MAV_DATA_STREAM_ALL=0, // 获取所有类别 数据流
   * MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
   * MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
   * MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
   * MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.
   * MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
   * MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot
   * MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot
   * MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot
   * MAV_DATA_STREAM_ENUM_END=13,
   */
  // 根据从Pixhawk请求的所需信息进行设置
  // const int maxStreams = 1;                                     // 遍历次数 （下面组合的长度）
  // const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_ALL}; // 请求的数据流组合 放到一个对象 后面进行遍历
  // const uint16_t MAVRates[maxStreams] = {0x01};                 // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹  既每秒发送一次
  const int maxStreams = 3;
  const uint8_t MAVStreams[maxStreams] = {
      MAV_DATA_STREAM_EXTENDED_STATUS,
      MAV_DATA_STREAM_POSITION,
      MAV_DATA_STREAM_EXTRA1};
  const uint16_t MAVRates[maxStreams] = {1, 1, 1};

  for (int i = 0; i < maxStreams; i++)
  {
    // 向飞控发送请求
    mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
    int len = mavlink_msg_to_send_buffer(buf, &msg);
    SWrite(buf, len, mavlinkSerial);
  }
  // printf("send_mav\n");
}

/**
 * @description: 向飞控请求指定的参数值
 * @param {char*} param_id 要请求的参数的名称
 */
void FoodCube::mav_parameter_data(const char *param_id)
{
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // 构造PARAM_REQUEST_READ消息
  mavlink_msg_param_request_read_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, param_id, -1);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 向飞控请求返航点的位置数据
 */
void FoodCube::mav_request_home_position()
{
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // 组装命令
  mavlink_command_long_t command = {0};
  command.command = MAV_CMD_GET_HOME_POSITION;
  command.confirmation = 0;
  command.param1 = 0; // 参数保留，不使用
  command.param2 = 0;
  command.param3 = 0;
  command.param4 = 0;
  command.param5 = 0;
  command.param6 = 0;
  command.param7 = 0;

  // 打包命令消息
  mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &command);

  // 将消息转换为字节流并发送
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 向飞控请求返航点的位置数据
 */
// void FoodCube::mav_request_home_position()
// {
//   mavlink_message_t msg;
//   uint8_t buf[MAVLINK_MAX_PACKET_LEN];

//   // 组装命令
//   mavlink_command_long_t command = {0};
//   command.command = MAV_CMD_REQUEST_MESSAGE; // 使用 MAV_CMD_REQUEST_MESSAGE 请求消息
//   command.confirmation = 0;
//   command.param1 = 49; // 请求的消息 ID，242 代表 Home 点位置
//   command.param2 = 0;  // 可选参数
//   command.param3 = 0;  // 可选参数
//   command.param4 = 0;  // 可选参数
//   command.param5 = 0;  // 可选参数
//   command.param6 = 0;  // 可选参数
//   command.param7 = 0;  // 可选参数

//   // 打包命令消息
//   mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &command);

//   // 将消息转换为字节流并发送
//   int len = mavlink_msg_to_send_buffer(buf, &msg);
//   SWrite(buf, len, mavlinkSerial);
// }

/**
 * @description: 解析mavlink数据流
 * @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
 */
void FoodCube::comm_receive(void (*pFun)(uint8_t))
{
  mavlink_message_t msg;
  mavlink_status_t status;
  switch (mavlinkSerial)
  {
  case 0:
    while (Serial.available() > 0)
    {                            // 判断串口缓存 是否有数据
      uint8_t c = Serial.read(); // 从缓存拿到数据
      pFun(c);
    }
    break;
  case 1:
    while (Serial1.available() > 0)
    {                             // 判断串口缓存 是否有数据
      uint8_t c = Serial1.read(); // 从缓存拿到数据
      pFun(c);
    }
    break;
  case 2:
    while (Serial2.available() > 0)
    {                             // 判断串口缓存 是否有数据
      uint8_t c = Serial2.read(); // 从缓存拿到数据
      pFun(c);
    }
    break;
  }
}

/**
 * @description: 写入航点 第一步 “向飞控发送航点数量”
 * @param {uint8_t} taskcount 航点数量
 */
void FoodCube::mav_mission_count(uint8_t taskcount)
{
  mavlink_message_t msg;               // mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
  // 设置飞行模式的数据包
  mavlink_msg_mission_count_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, taskcount);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  // 通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 等待飞控反馈之后 写入指定航点
 * @param {uint8_t} seq 航点序列
 * @param {uint8_t} frame 通常第一个是home航点0 绝对海拔  后面的固定的全是 3 相对高度
 * @param {uint8_t} command 航点的类型 如：16 是waypoint模式
 * @param {uint8_t} current 固定给0
 * @param {uint8_t} autocontinue 是否自动飞下一个航点
 * @param {double} param1
 * @param {double} param2
 * @param {double} param3
 * @param {double} param4
 * @param {double} x
 * @param {double} y
 * @param {double} z
 */
void FoodCube::mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z)
{
  mavlink_message_t msg;               // mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
  // 写入航点数据包
  mavlink_msg_mission_item_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  // 通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

void FoodCube::mav_send_text(uint8_t severity, const char *text)
{
  return;
  mavlink_message_t msg;               // mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
  memset(&msg, 0, sizeof(mavlink_message_t));
  mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, severity, text);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  printf("Send:");
  for (int i = 0; i < len; i++)
  {
    printf("0x%02X ", buf[i]);
  }
  printf(" \n");
  // 通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 油门控制
 * @param {uint16_t} chan[] 四个通道
 */
void FoodCube::mav_channels_override(uint16_t chan[])
{
  mavlink_message_t msg;               // mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
  // 控制油门
  mavlink_msg_rc_channels_override_pack(0xFF, 0xBE, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  // 通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @brief 发送MAVLink命令
 * 该函数通过串口发送一个MAVLink `COMMAND_LONG` 命令。此命令可以用于向飞控发送各种控制命令，
 * 包括但不限于模式切换、校准、任务启动等。
 * @param msg_cmd 参考传递的 `mavlink_command_long_t` 类型的 MAVLink 命令结构体。
 */
void FoodCube::mav_send_command(mavlink_command_long_t &msg_cmd)
{
  mavlink_message_t msg;               // mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
  mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  // 通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @brief 发送 COMMAND_ACK 消息
 * 该函数用于发送一个 `COMMAND_ACK` 消息，以响应飞控发送的命令确认请求。该消息通常用于通知飞控某个命令
 * 是否已被接收和处理，以及处理的结果状态。
 * @param command 表示要响应的命令 ID，通常与接收到的命令相对应。
 * @param result 表示命令的处理结果。常用值包括：
 *               - 0: MAV_RESULT_ACCEPTED (命令成功接受)
 *               - 1: MAV_RESULT_TEMPORARILY_REJECTED (命令被暂时拒绝)
 *               - 2: MAV_RESULT_DENIED (命令被拒绝)
 *               - 3: MAV_RESULT_UNSUPPORTED (命令不受支持)
 *               - 4: MAV_RESULT_FAILED (命令执行失败)
 */
void FoodCube::sendCommandAck(uint16_t command, uint8_t result)
{
  mavlink_message_t msg;               // MAVLink 信息
  uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送缓存
  // 打包 COMMAND_ACK 消息
  mavlink_msg_command_ack_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, command, result);
  // 将 MAVLink 消息转换为字节流
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  // 通过串口发送数据
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @brief 向飞控写入参数
 * @param param_id 参数名称（例如 "COMPASS_OFS_X"）
 * @param value 参数值（浮点数）
 */
void FoodCube::setParam(const char *param_id, float value)
{
  mavlink_message_t msg;
  mavlink_param_set_t param_set;

  // 设置参数
  param_set.param_value = value;
  strncpy(param_set.param_id, param_id, sizeof(param_set.param_id));
  param_set.param_id[sizeof(param_set.param_id) - 1] = '\0'; // 确保字符串以null结尾
  param_set.target_system = 1;                               // 目标系统 ID
  param_set.target_component = 1;                            // 目标组件 ID

  // 打包参数设置消息
  mavlink_msg_param_set_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &param_set);

  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  int len = mavlink_msg_to_send_buffer(buf, &msg);

  // 通过串口发送数据
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @brief 向飞控发送导航目标位置（经纬度 + 高度）
 * @param lat_deg 纬度（单位：度）
 * @param lon_deg 经度（单位：度）
 * @param alt_m 相对高度（单位：米，相对于起飞点）
 */
void FoodCube::sendSetPositionTargetGlobalInt(double lat_deg, double lon_deg, double alt_m)
{
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_set_position_target_global_int_t sp = {};
  sp.time_boot_ms = millis();
  sp.target_system = 1;    // 目标系统 ID（可根据需要调整）
  sp.target_component = 1; // 目标组件 ID（通常为飞控主组件）

  sp.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;

  // 设置 type_mask，仅启用位置控制字段，其它字段被忽略
  sp.type_mask =
      (1 << 3) |  // 忽略 vx
      (1 << 4) |  // 忽略 vy
      (1 << 5) |  // 忽略 vz
      (1 << 6) |  // 忽略 afx
      (1 << 7) |  // 忽略 afy
      (1 << 8) |  // 忽略 afz
      (1 << 10) | // 忽略 yaw
      (1 << 11);  // 忽略 yaw_rate

  sp.lat_int = lat_deg * 1e7;
  sp.lon_int = lon_deg * 1e7;
  sp.alt = alt_m;

  // 打包消息 MAVLINK_SYSTEM_ID 255  MAV_COMP_ID 0
  mavlink_msg_set_position_target_global_int_encode(255, 0, &msg, &sp);

  // 转换为字节流并通过串口发送
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  SWrite(buf, len, mavlinkSerial);
}