#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) { // 拼接音量控制前缀，例如 "[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(result); // 低位 互换 bytes[len + 1] = static_cast(result >> 8); // 高位 互换 /* //发送日志 printf("SendToCamera:"); for (int i=0;i 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, ¶m_set); uint8_t buf[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buf, &msg); // 通过串口发送数据 SWrite(buf, len, mavlinkSerial); }