1049 lines
33 KiB
C++
1049 lines
33 KiB
C++
#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 = "wxsky.com"; // 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
|
||
/*触发发送 主题*/
|
||
// 0:对频信息
|
||
String topicHandle[] = {"crosFrequency"};
|
||
boolean isPush = false; // 记得删除 板子按钮状态 ps:D3引脚下拉微动开关
|
||
|
||
/*异步线程对象*/
|
||
Ticker pubTicker; // 定时发布主题 线程
|
||
Ticker mavTicker; // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||
// Ticker chanTicker; //定时向飞控 发送油门指定
|
||
|
||
/**
|
||
* @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 == "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:切自动
|
||
{
|
||
// 切模式
|
||
set_mode(AUTO); // 设置飞控为自动模式
|
||
// 油门
|
||
delay(500);
|
||
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
|
||
fc.mav_channels_override(chan); // 控制油门
|
||
}
|
||
}
|
||
else if (key == "unlock") // 解锁
|
||
{
|
||
|
||
set_mode(ALT_HOLD); // 设置飞控为定高模式
|
||
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
|
||
lock_or_unlock(false); // 解锁
|
||
}
|
||
else if (key == "lock") // 加锁
|
||
{
|
||
// 油门
|
||
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
|
||
fc.mav_channels_override(chan); // 控制油门
|
||
// 加锁
|
||
lock_or_unlock(true);
|
||
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
|
||
set_mode(ALT_HOLD); // 设置飞控为定高模式
|
||
}
|
||
else if (key == "guidedMode") // 指点
|
||
{
|
||
// 设置飞控为 Guided 模式
|
||
set_mode(GUIDED);
|
||
// 可以根据需要调整延迟时间，确保模式切换生效
|
||
delay(2000);
|
||
// 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>();
|
||
}
|
||
// 飞至指定位置
|
||
fly_to_localtion(lat, lon, alt);
|
||
}
|
||
else if (key == "autoMode") // 自动auto模式
|
||
{
|
||
// 切模式
|
||
set_mode(AUTO); // 设置飞控为定高模式
|
||
// 油门
|
||
delay(500);
|
||
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); // 控制油门
|
||
delay(500);
|
||
// 切模式
|
||
set_mode(LOITER); // 设置飞控为定高模式
|
||
// 油门
|
||
delay(500);
|
||
fc.mav_channels_override(chan); // 控制油门
|
||
}
|
||
else if (key == "holdAltMode") // 定高
|
||
{
|
||
// 油门
|
||
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
|
||
fc.mav_channels_override(chan); // 控制油门
|
||
delay(500);
|
||
// 切模式
|
||
set_mode(ALT_HOLD);
|
||
// 油门
|
||
delay(500);
|
||
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");
|
||
if (motocontrol.getcontrolstatus().is_autogoodsdown)
|
||
up_action(SPEED_BTN_FAST);
|
||
}
|
||
break;
|
||
case 1:
|
||
{
|
||
// 放钩
|
||
}
|
||
break;
|
||
case 2:
|
||
{
|
||
// 暂停
|
||
ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE");
|
||
if (motocontrol.getcontrolstatus().is_autogoodsdown)
|
||
Hook_stop();
|
||
}
|
||
break;
|
||
case 3:
|
||
{
|
||
// 继续
|
||
ESP_LOGD(MOUDLENAME, "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) // 如果正在写入状态 跳出
|
||
{
|
||
fc.logln("正在写航点"); // 提示正在写入中
|
||
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默认状态
|
||
fc.logln("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默认状态
|
||
fc.logln("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;
|
||
}
|
||
fc.logln("frame--");
|
||
fc.logln(frame);
|
||
// 写入航点
|
||
fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
|
||
fc.futureSeq++; // 下一个航点序号
|
||
}
|
||
delay(20);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @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_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("[v1]已解锁");
|
||
}
|
||
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("[v1]已加锁");
|
||
}
|
||
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]);
|
||
}
|
||
}
|
||
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_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_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结尾
|
||
if (topicPubMsg[18] != buf)
|
||
{
|
||
topicPubMsg[18] = 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_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;
|
||
|
||
case MAVLINK_MSG_ID_PARAM_VALUE: // #22:获取飞控参数信息
|
||
{
|
||
mavlink_param_value_t param_value;
|
||
mavlink_msg_param_value_decode(&msg, ¶m_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 (topicPubMsg[i] != "")
|
||
{
|
||
doc[topicPub[i]] = topicPubMsg[i];
|
||
topicPubMsg[i] = ""; // 重置
|
||
}
|
||
}
|
||
}
|
||
// 将JSON对象序列化为JSON字符串
|
||
String jsonString;
|
||
serializeJson(doc, jsonString);
|
||
fc.pubMQTTmsg("planeState", jsonString);
|
||
}
|
||
|
||
/**
|
||
* @description: 刷新各请求
|
||
*/
|
||
void refreshRequest()
|
||
{
|
||
fc.mav_request_home_position();
|
||
delay(50);
|
||
fc.mav_request_data(); // 请求 设定飞控输出数据流内容
|
||
delay(50);
|
||
fc.mav_parameter_data("BATT_CAPACITY"); // 请求飞控 返回参数 电池容量 值
|
||
delay(50);
|
||
fc.mav_parameter_data("WPNAV_SPEED"); // 请求飞控 返回参数 返航速度
|
||
delay(50);
|
||
}
|
||
|
||
/**
|
||
* @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 飞到指定位置
|
||
* @param lat 维度
|
||
* @param lon 经度
|
||
* @param alt 高度
|
||
*/
|
||
void fly_to_localtion(double lat, double lon, double alt)
|
||
{
|
||
mavlink_command_long_t msg_cmd = {};
|
||
msg_cmd.command = MAV_CMD_NAV_TAKEOFF;
|
||
msg_cmd.param1 = 0;
|
||
msg_cmd.param2 = 0;
|
||
msg_cmd.param3 = 0;
|
||
msg_cmd.param4 = 0;
|
||
msg_cmd.param5 = lat;
|
||
msg_cmd.param6 = lon;
|
||
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);
|
||
} |