[feat] 融合新改的mqtt代码
将mqtt和小田代码单独放到commer.cpp里面，主程序调用 仅融合,代码未完善 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
This commit is contained in:
parent
af56449988
commit
f3583e17e5
598
src/commser.cpp
Normal file
598
src/commser.cpp
Normal file
@ -0,0 +1,598 @@
|
|||||||
|
#include "commser.h"
|
||||||
|
#include "motocontrol.h"
|
||||||
|
//////////////////////////////MQTT_语音_MAVLINK 部分
|
||||||
|
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 = "szdot"; //wifi帐号
|
||||||
|
//char* password = "Ttaj@#*.com"; //wifi密码
|
||||||
|
char* ssid = "flicube"; //wifi帐号
|
||||||
|
char* password = "fxmf0622"; //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:{经度，维度，海拔高度}
|
||||||
|
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position" };
|
||||||
|
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //登记 json成员总数
|
||||||
|
String topicPubMsg[16]; //登记 json成员的值 对应topicPub
|
||||||
|
String oldMsg[16]; //记录旧的值 用来对比有没有更新
|
||||||
|
/*触发发送 主题*/
|
||||||
|
//0:对频信息
|
||||||
|
String topicHandle[] = { "crosFrequency" };
|
||||||
|
boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微动开关
|
||||||
|
|
||||||
|
/*异步线程对象*/
|
||||||
|
Ticker pubTicker(pubThread,1000); //定时发布主题 线程
|
||||||
|
Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||||
|
Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程
|
||||||
|
|
||||||
|
//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 == "setPlaneState") { //设置飞机状态
|
||||||
|
/*
|
||||||
|
*其中topicPubMsg[10]既飞机状态的值
|
||||||
|
*二进制0000 0000 0000 0000
|
||||||
|
*第0位：初始状态
|
||||||
|
*第1位：是否正在写入航线
|
||||||
|
*第2位：是否已经写入航点
|
||||||
|
*第3位：是否正在解锁
|
||||||
|
*第4位：解锁是否成功
|
||||||
|
*第5位：执行航点任务
|
||||||
|
*第6位：起飞
|
||||||
|
*第7位: 悬停
|
||||||
|
*第8位：降落
|
||||||
|
*第9位：返航
|
||||||
|
*第10位：航点继续
|
||||||
|
*第11位：磁罗盘校准
|
||||||
|
*/
|
||||||
|
String todoJson = value; //转换值
|
||||||
|
/* json 反序列化 */
|
||||||
|
DynamicJsonDocument doc(500);
|
||||||
|
deserializeJson(doc, todoJson);
|
||||||
|
JsonObject obj = doc.as<JsonObject>();
|
||||||
|
uint8_t n = obj["bit"]; //状态位数
|
||||||
|
uint8_t state = obj["state"]; //标记飞机状态 0 or 1
|
||||||
|
uint8_t count = obj["count"]; //传过来
|
||||||
|
//解构val数组参数
|
||||||
|
uint16_t param[count];
|
||||||
|
for (int i = 0; i < count; i++) {
|
||||||
|
param[i] = obj["param"][i];
|
||||||
|
}
|
||||||
|
//标记飞机状态
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
|
||||||
|
//飞控执行
|
||||||
|
if (n == 3) { //3操作飞机加解锁
|
||||||
|
uint16_t chan[] = { 1500, 1500, 1100, 1500 }; //加解锁 油门到底
|
||||||
|
fc.mav_channels_override(chan); //控制油门
|
||||||
|
fc.mav_set_mode(2); //飞控设置成AltHold定高
|
||||||
|
fc.mav_command(n, param);
|
||||||
|
} else {
|
||||||
|
uint16_t chan[] = { 1500, 1500, 1500, 1500 }; //除了加解锁模式 油门全部控制居中
|
||||||
|
fc.mav_channels_override(chan); //控制油门
|
||||||
|
}
|
||||||
|
if (n == 6) { //6测试起飞
|
||||||
|
fc.mav_set_mode(4); //飞控设置成Guided引导模式
|
||||||
|
fc.mav_command(n, param); //起飞
|
||||||
|
} else if (n == 7) { //7 悬停
|
||||||
|
fc.mav_set_mode(5); //飞控设置成Loiter留待模式
|
||||||
|
} else if (n == 5) { //5 航点执行
|
||||||
|
fc.mav_set_mode(3); //飞控设置成auto自动模式
|
||||||
|
} else if (n == 8) { //8降落*
|
||||||
|
fc.mav_command(n, param);
|
||||||
|
} else if (n == 9) { //9返航
|
||||||
|
fc.mav_set_mode(6); //飞控设置成RTL返航
|
||||||
|
} else if (n == 11) { //11磁罗盘校准
|
||||||
|
fc.mav_command(n, param);
|
||||||
|
}
|
||||||
|
} else if (key == "getPlaneState") { //获取飞机状态
|
||||||
|
fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //终端主动get飞机状态
|
||||||
|
} else if (key == "resetState") { //恢复飞机为初始状态
|
||||||
|
String todo = value; //转换值
|
||||||
|
topicPubMsg[10] = todo; //恢复初始状态
|
||||||
|
} else if (key == "chan1") {
|
||||||
|
uint16_t todo = value; //转换值
|
||||||
|
fc.channels[0] = todo; //恢复初始状态
|
||||||
|
fc.mav_channels_override(fc.channels); //油门控制
|
||||||
|
} else if (key == "chan2") {
|
||||||
|
uint16_t todo = value; //转换值
|
||||||
|
fc.channels[1] = todo; //恢复初始状态
|
||||||
|
fc.mav_channels_override(fc.channels); //油门控制
|
||||||
|
} else if (key == "chan3") {
|
||||||
|
uint16_t todo = value; //转换值
|
||||||
|
fc.channels[2] = todo; //恢复初始状态
|
||||||
|
fc.mav_channels_override(fc.channels); //油门控制
|
||||||
|
} else if (key == "chan4") {
|
||||||
|
uint16_t todo = value; //转换值
|
||||||
|
fc.channels[3] = todo; //恢复初始状态
|
||||||
|
fc.mav_channels_override(fc.channels); //油门控制
|
||||||
|
} else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
|
||||||
|
uint16_t todo = value;
|
||||||
|
switch(todo){
|
||||||
|
case 0:
|
||||||
|
{
|
||||||
|
//收钩
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
{
|
||||||
|
//放钩
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
{
|
||||||
|
//暂停
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
{
|
||||||
|
//继续
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
{
|
||||||
|
//重置重量
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
//改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态
|
||||||
|
fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}");//发送正在写入的飞机状态
|
||||||
|
// 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; //锁定写入状态
|
||||||
|
//监听飞控航点写入情况
|
||||||
|
while (true) {
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点
|
||||||
|
fc.comm_receive(mavlink_receiveCallback);
|
||||||
|
if (fc.missionArkType == 0) { //写入成功
|
||||||
|
fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态
|
||||||
|
fc.logln("misson_bingo...");
|
||||||
|
//改变飞机状态
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态
|
||||||
|
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); //结束写入状态
|
||||||
|
//当有成果反馈之后 初始化下列数据
|
||||||
|
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 != "") {
|
||||||
|
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(200);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @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;
|
||||||
|
// 尝试从数据流里 解析数据
|
||||||
|
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
|
||||||
|
// 通过msgid来判断 数据流的类别
|
||||||
|
char buf[10];
|
||||||
|
switch (msg.msgid) {
|
||||||
|
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) { //从心跳里面 判断已经解锁
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
|
||||||
|
} else { //心跳里面 判断没有解锁
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
|
||||||
|
if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点
|
||||||
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态
|
||||||
|
} else {
|
||||||
|
topicPubMsg[10] = "1"; //没写航点 设置为初始化状态
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//从心跳里面 判断飞机当前的模式
|
||||||
|
switch (heartbeat.custom_mode) {
|
||||||
|
case 0:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "Stabilize自稳";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "AltHold定高";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "Auto自动";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "Guided引导";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "Loiter留待";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 6:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "RTL返航";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 9:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "Land降落";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 16:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "PosHold定点";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
topicPubMsg[12] = "Unknown未知";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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);
|
||||||
|
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_PARAM_VALUE: // #22: PARAM_VALUE
|
||||||
|
{
|
||||||
|
mavlink_param_value_t param_value;
|
||||||
|
mavlink_msg_param_value_decode(&msg, ¶m_value);
|
||||||
|
}
|
||||||
|
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.relative_alt / 1000);
|
||||||
|
if (topicPubMsg[4] != buf) {
|
||||||
|
topicPubMsg[4] = buf;
|
||||||
|
}
|
||||||
|
//{经度，维度，海拔高度}
|
||||||
|
sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}",
|
||||||
|
global_position_int.lon,
|
||||||
|
global_position_int.lat,
|
||||||
|
(double)global_position_int.alt / 1000);
|
||||||
|
if (topicPubMsg[15] != buf) {
|
||||||
|
topicPubMsg[15] = buf;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
// 纬度
|
||||||
|
sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
|
||||||
|
if (topicPubMsg[7] != buf) {
|
||||||
|
topicPubMsg[7] = buf;
|
||||||
|
}
|
||||||
|
// 经度
|
||||||
|
sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
|
||||||
|
if (topicPubMsg[8] != buf) {
|
||||||
|
topicPubMsg[8] = buf;
|
||||||
|
}
|
||||||
|
// 卫星状态
|
||||||
|
switch (gps_raw_int.fix_type) {
|
||||||
|
case 0:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "No Fix";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "No Fix";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "Fix 2D";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "Fix 3D";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "Low GPS";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "Float";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
topicPubMsg[9] = "Fixed";
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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("MsgID:");
|
||||||
|
fc.logln(msg.msgid);
|
||||||
|
fc.logln("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("MsgID:");
|
||||||
|
fc.logln(msg.msgid);
|
||||||
|
fc.logln("re:");
|
||||||
|
fc.logln(mission_ark.type);
|
||||||
|
/*记录航点的写入状态 */
|
||||||
|
fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败
|
||||||
|
/*当有成果反馈之后 初始化下列数据*/
|
||||||
|
fc.writeState = false; //是否是写入状态
|
||||||
|
fc.writeSeq = -1; //飞控反馈 需写入航点序列号
|
||||||
|
fc.futureSeq = 0; //记录将来要写入 航点序列号
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @description: 发送主题线程
|
||||||
|
*/
|
||||||
|
void pubThread() {
|
||||||
|
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
|
||||||
|
// 创建一个JSON对象
|
||||||
|
DynamicJsonDocument doc(2000); // 缓冲区
|
||||||
|
//遍历 有更新的数据 组成一个json对象
|
||||||
|
for (int i = 0; i < topicPubCount; i++) {
|
||||||
|
if (topicPubMsg[i] != oldMsg[i]) {
|
||||||
|
if (i == 0) { //心跳包 每每向心跳主题发布信息
|
||||||
|
//启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
|
||||||
|
if (fc.getIsInit()) {
|
||||||
|
fc.setIsInit(false);
|
||||||
|
fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容
|
||||||
|
}
|
||||||
|
//设置对象成员 ps:心跳
|
||||||
|
doc[topicPub[0]] = topicPubMsg[0];
|
||||||
|
} else { //非心跳 有更新 录入成员
|
||||||
|
doc[topicPub[i]] = topicPubMsg[i];
|
||||||
|
oldMsg[i] = topicPubMsg[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// 将JSON对象序列化为JSON字符串
|
||||||
|
String jsonString;
|
||||||
|
serializeJson(doc, jsonString);
|
||||||
|
fc.pubMQTTmsg("planeState", jsonString);
|
||||||
|
/*更新4G网络测速ping值*/
|
||||||
|
//pingNetTest();
|
||||||
|
|
||||||
|
//八达岭需求 过后删除
|
||||||
|
fc.pubMQTTmsg("heartBeat", topicPubMsg[0]);
|
||||||
|
fc.pubMQTTmsg("position", topicPubMsg[15]);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
|
||||||
|
*/
|
||||||
|
void flashThread() {
|
||||||
|
if (digitalRead(23) == LOW) {
|
||||||
|
if (isPush) { //点击之后
|
||||||
|
//请求注册 ps:发送esp8266的物理地址 到对频主题
|
||||||
|
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
|
||||||
|
}
|
||||||
|
isPush = false; //复位按钮
|
||||||
|
} else {
|
||||||
|
//FLASH按下状态
|
||||||
|
isPush = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||||
|
*/
|
||||||
|
void mavThread() {
|
||||||
|
fc.mav_request_data();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @description: 向飞控 发送油门指令
|
||||||
|
*/
|
||||||
|
void chanThread() {
|
||||||
|
//mav_channels_override(channels);
|
||||||
|
}
|
||||||
21
src/commser.h
Normal file
21
src/commser.h
Normal file
@ -0,0 +1,21 @@
|
|||||||
|
#ifndef COMMSER_H
|
||||||
|
#define COMMSER_H
|
||||||
|
|
||||||
|
#include "Arduino.h"
|
||||||
|
#include <Ticker.h> //调用Ticker.h库
|
||||||
|
#include "FoodDeliveryBase.h"
|
||||||
|
#include "config.h"
|
||||||
|
|
||||||
|
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
|
||||||
|
void mavThread();
|
||||||
|
void pubThread();
|
||||||
|
void flashThread() ;
|
||||||
|
void writeRoute(String topicStr);
|
||||||
|
void mavlink_receiveCallback(uint8_t c) ;
|
||||||
|
extern String topicPub[];
|
||||||
|
extern int topicPubCount;
|
||||||
|
extern FoodCube fc; //创建项目对象
|
||||||
|
extern Ticker pubTicker; //定时发布主题 线程
|
||||||
|
extern Ticker mavTicker; //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||||
|
extern Ticker flashTicker; //单片机主动 按钮主动发布主题 线程
|
||||||
|
#endif
|
||||||
19
src/config.h
19
src/config.h
@ -54,3 +54,22 @@ enum HookStatus
|
|||||||
HS_Locked, // 已到顶部锁定
|
HS_Locked, // 已到顶部锁定
|
||||||
HS_Stop // 已停止
|
HS_Stop // 已停止
|
||||||
};
|
};
|
||||||
|
enum Initstatus
|
||||||
|
{
|
||||||
|
IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线
|
||||||
|
IS_Start, // 0. 刚上电
|
||||||
|
IS_Wait_Locked,
|
||||||
|
IS_LengthZero, // 1.已确定零点
|
||||||
|
IS_begin_WeightZero,
|
||||||
|
IS_Wait_WeightZero,
|
||||||
|
IS_WeightZero, // 2.已确定称重传感器0点
|
||||||
|
IS_InStoreLocked, // 3. 挂钩入仓顶锁定
|
||||||
|
IS_CheckWeightZero, // 检查称重传感器是否归0
|
||||||
|
IS_OK, // 4.所有初始化已经OK，可以正常操作
|
||||||
|
IS_Error // 5.初始化遇到错误
|
||||||
|
} ;
|
||||||
|
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线
|
||||||
|
const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停
|
||||||
|
const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态
|
||||||
|
const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
|
||||||
|
const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下
|
||||||
|
|||||||
742
src/main.cpp
742
src/main.cpp
@ -11,6 +11,7 @@
|
|||||||
#include <ESP32Servo.h>
|
#include <ESP32Servo.h>
|
||||||
#include "FoodDeliveryBase.h"
|
#include "FoodDeliveryBase.h"
|
||||||
#include <Preferences.h>
|
#include <Preferences.h>
|
||||||
|
#include "commser.h"
|
||||||
|
|
||||||
// 角度传感器
|
// 角度传感器
|
||||||
// 收放线电机控制
|
// 收放线电机控制
|
||||||
@ -83,73 +84,15 @@ enum Weightalign_status
|
|||||||
} _weightalign_status;
|
} _weightalign_status;
|
||||||
unsigned long _weightalign_begintm; //校准开始时间
|
unsigned long _weightalign_begintm; //校准开始时间
|
||||||
// 需要做的初始化工作
|
// 需要做的初始化工作
|
||||||
enum Initstatus
|
|
||||||
{
|
Initstatus initstatus;
|
||||||
IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线
|
|
||||||
IS_Start, // 0. 刚上电
|
|
||||||
IS_Wait_Locked,
|
|
||||||
IS_LengthZero, // 1.已确定零点
|
|
||||||
IS_begin_WeightZero,
|
|
||||||
IS_Wait_WeightZero,
|
|
||||||
IS_WeightZero, // 2.已确定称重传感器0点
|
|
||||||
IS_InStoreLocked, // 3. 挂钩入仓顶锁定
|
|
||||||
IS_CheckWeightZero, // 检查称重传感器是否归0
|
|
||||||
IS_OK, // 4.所有初始化已经OK，可以正常操作
|
|
||||||
IS_Error // 5.初始化遇到错误
|
|
||||||
} initstatus;
|
|
||||||
unsigned long _tm_waitinit;
|
unsigned long _tm_waitinit;
|
||||||
|
|
||||||
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线
|
|
||||||
const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停
|
|
||||||
const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态
|
|
||||||
const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
|
|
||||||
const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下
|
|
||||||
|
|
||||||
///////////////////////
|
|
||||||
|
|
||||||
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
|
||||||
/*项目对象*/
|
|
||||||
//char* ssid = "szdot"; //wifi帐号
|
|
||||||
//char* password = "Ttaj@#*.com"; //wifi密码
|
|
||||||
char* ssid = (char*)"fxmf_sc01"; //wifi帐号 "flicube";// "fxmf_sc01"
|
|
||||||
char* password = (char*)"12345678"; //wifi密码 "fxmf0622"; //"12345678"
|
|
||||||
char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //114.115.137.239 mqtt地址
|
|
||||||
int mqttPort = 1883; //mqtt端口
|
|
||||||
char* mqttName = (char*)"admin"; //mqtt帐号
|
|
||||||
char* mqttPassword = (char*)"123456"; //mqtt密码
|
|
||||||
uint8_t mavlinkSerial = 2; //飞控占用的串口号
|
|
||||||
uint8_t voiceSerial = 1; //声音模块占用串口
|
|
||||||
char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip
|
|
||||||
uint32_t udpServerPort = 37260; //云台相机端口
|
|
||||||
|
|
||||||
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
|
|
||||||
void mavThread();
|
|
||||||
void pubThread();
|
|
||||||
void flashThread() ;
|
|
||||||
void writeRoute(String topicStr);
|
|
||||||
void mavlink_receiveCallback(uint8_t c) ;
|
|
||||||
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort); //创建项目对象
|
|
||||||
|
|
||||||
/*订阅 主题*/
|
|
||||||
//0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 8:钩子控制
|
|
||||||
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController", "cmd" }; //订阅主题
|
|
||||||
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数
|
|
||||||
/*有更新主动发送 主题*/
|
|
||||||
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度)
|
|
||||||
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode","loadweight","hookstatus","position"};
|
|
||||||
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数
|
|
||||||
String topicPubMsg[16]; //发送数据存放 对应topicPub
|
|
||||||
String oldMsg[16]; //记录旧的数据 用来对比有没有更新
|
|
||||||
/*触发发送 主题*/
|
|
||||||
//0:对频信息
|
|
||||||
String topicHandle[] = { "crosFrequency" };
|
|
||||||
boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微动开关
|
|
||||||
|
|
||||||
/*异步线程对象*/
|
|
||||||
Ticker pubTicker(pubThread,1000); //定时发布主题 线程
|
|
||||||
Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
|
||||||
//Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程
|
|
||||||
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
@ -208,15 +151,15 @@ void setup()
|
|||||||
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射
|
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射
|
||||||
fc.playText("开始启动");
|
fc.playText("开始启动");
|
||||||
fc.connectWifi(); //连接wifi
|
fc.connectWifi(); //连接wifi
|
||||||
// fc.playText("正在连接服务器");
|
fc.playText("正在连接服务器");
|
||||||
// fc.connectMqtt(topicSub, topicSubCount); //连接mqtt
|
fc.connectMqtt(topicPub, topicPubCount); //连接mqtt
|
||||||
fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调
|
fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调
|
||||||
fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
|
fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
|
||||||
|
|
||||||
/*异步线程*/
|
/*异步线程*/
|
||||||
pubTicker.start(); //定时 发布主题
|
pubTicker.start(); //定时 发布主题
|
||||||
mavTicker.start(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
mavTicker.start(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||||
//flashTicker.start(); //监听 按flash键时 主动发布对频主题
|
flashTicker.start(); //监听 按flash键时 主动发布对频主题
|
||||||
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
||||||
|
|
||||||
|
|
||||||
@ -281,8 +224,8 @@ void showinfo()
|
|||||||
// if (pullweight > 10)
|
// if (pullweight > 10)
|
||||||
// printf("PullWeight:%d\n", pullweight); //发送重量到mqtt
|
// printf("PullWeight:%d\n", pullweight); //发送重量到mqtt
|
||||||
|
|
||||||
topicPubMsg[14]=motocontrol.gethooktatus_str() ;
|
//topicPubMsg[14]=motocontrol.gethooktatus_str() ;
|
||||||
topicPubMsg[13]=pullweight;
|
//topicPubMsg[13]=pullweight;
|
||||||
|
|
||||||
// control_status_t cs=motocontrol.getcontrolstatus() ;
|
// control_status_t cs=motocontrol.getcontrolstatus() ;
|
||||||
|
|
||||||
@ -468,8 +411,8 @@ void set_locked(bool locked)
|
|||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
tksendinfo.update(); // 定时发送信息任务
|
tksendinfo.update(); // 定时发送信息任务
|
||||||
pubTicker.update(); //定时 发布主题
|
//pubTicker.update(); //定时 发布主题
|
||||||
mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
//mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||||
|
|
||||||
|
|
||||||
// sercomm.getcommand(); // 得到控制命令
|
// sercomm.getcommand(); // 得到控制命令
|
||||||
@ -509,6 +452,8 @@ void loop()
|
|||||||
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
||||||
/*从飞控拿数据*/
|
/*从飞控拿数据*/
|
||||||
fc.comm_receive(mavlink_receiveCallback);
|
fc.comm_receive(mavlink_receiveCallback);
|
||||||
|
/*保持mqtt心跳*/
|
||||||
|
fc.mqttLoop(topicPub, topicPubCount);
|
||||||
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
@ -535,9 +480,11 @@ void Task1(void *pvParameters)
|
|||||||
|
|
||||||
/*保持mqtt心跳*/
|
/*保持mqtt心跳*/
|
||||||
// fc.mqttLoop(topicSub, topicSubCount);
|
// fc.mqttLoop(topicSub, topicSubCount);
|
||||||
if (fc.checkWiFiStatus())
|
|
||||||
|
///px1
|
||||||
|
// if (fc.checkWiFiStatus())
|
||||||
/*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
|
/*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
|
||||||
fc.mqttLoop(topicSub, topicSubCount);
|
// fc.mqttLoop(topicSub, topicSubCount);
|
||||||
vTaskDelay(10);
|
vTaskDelay(10);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -775,7 +722,8 @@ void btn_presssatonce()
|
|||||||
ESP_LOGD(MOUDLENAME,"UP_presssatonce");
|
ESP_LOGD(MOUDLENAME,"UP_presssatonce");
|
||||||
led_show(255,255, 255); // 高亮一下
|
led_show(255,255, 255); // 高亮一下
|
||||||
fc.playText("发送对频信息");
|
fc.playText("发送对频信息");
|
||||||
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
|
///px1
|
||||||
|
//fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
|
||||||
}
|
}
|
||||||
// 收线按钮-长按
|
// 收线按钮-长按
|
||||||
void upbtn_pressstart()
|
void upbtn_pressstart()
|
||||||
@ -865,658 +813,4 @@ void testbtn_click()
|
|||||||
|
|
||||||
//////////////////////////////MQTT_语音_MAVLINK 部分
|
//////////////////////////////MQTT_语音_MAVLINK 部分
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: mqtt订阅主题 收到信息 的回调函数
|
|
||||||
* @param {char*} topic 主题名称
|
|
||||||
* @param {byte*} topic 订阅获取的内容
|
|
||||||
* @param {unsigned int} length 内容的长度
|
|
||||||
*/
|
|
||||||
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
|
||||||
/*获取 "订阅主题/macadd" 截取/macadd后的长度 */
|
|
||||||
int count = strlen(topic);
|
|
||||||
String cutTopic = ""; //记录订阅主题
|
|
||||||
for (int i = 0; i < count - 13; i++) {
|
|
||||||
cutTopic += topic[i];
|
|
||||||
}
|
|
||||||
/*解构mqtt发过来的内容*/
|
|
||||||
String topicStr = "";
|
|
||||||
for (int i = 0; i < length; i++) {
|
|
||||||
topicStr += (char)payload[i];
|
|
||||||
}
|
|
||||||
/*订阅回调主体*/
|
|
||||||
if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss
|
|
||||||
writeRoute(topicStr); //写入航点
|
|
||||||
} else
|
|
||||||
if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState
|
|
||||||
/*
|
|
||||||
*其中topicPubMsg[10]既飞机状态的值
|
|
||||||
*二进制0000 0000 0000 0000
|
|
||||||
*第0位：初始状态
|
|
||||||
*第1位：是否正在写入航线
|
|
||||||
*第2位：是否已经写入航点
|
|
||||||
*第3位：是否正在解锁
|
|
||||||
*第4位：解锁是否成功
|
|
||||||
*第5位：执行航点任务
|
|
||||||
*第6位：起飞
|
|
||||||
*第7位: 悬停
|
|
||||||
*第8位：降落
|
|
||||||
*第9位：返航
|
|
||||||
*第10位：航点继续
|
|
||||||
*第11位：磁罗盘校准
|
|
||||||
*/
|
|
||||||
//解构参数
|
|
||||||
DynamicJsonDocument doc(0x2FFF);
|
|
||||||
deserializeJson(doc, topicStr);
|
|
||||||
JsonObject obj = doc.as<JsonObject>();
|
|
||||||
uint8_t n = obj["bit"]; //状态位数
|
|
||||||
uint8_t state = obj["state"]; //标记飞机状态 0 or 1
|
|
||||||
uint8_t count = obj["count"]; //传过来
|
|
||||||
//解构val数组参数
|
|
||||||
uint16_t param[count];
|
|
||||||
for (int i = 0; i < count; i++) {
|
|
||||||
param[i] = obj["param"][i];
|
|
||||||
}
|
|
||||||
//标记飞机状态
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
|
|
||||||
//飞控执行
|
|
||||||
if (n == 3) { //3操作飞机加解锁
|
|
||||||
uint16_t chan[] = { 1500, 1500, 1100, 1500 }; //加解锁 油门到底
|
|
||||||
fc.mav_channels_override(chan); //控制油门
|
|
||||||
fc.mav_set_mode(2); //飞控设置成AltHold定高
|
|
||||||
fc.mav_command(n, param);
|
|
||||||
} else {
|
|
||||||
uint16_t chan[] = { 1500, 1500, 1500, 1500 }; //除了加解锁模式 油门全部控制居中
|
|
||||||
fc.mav_channels_override(chan); //控制油门
|
|
||||||
}
|
|
||||||
if (n == 6) { //6测试起飞
|
|
||||||
fc.mav_set_mode(4); //飞控设置成Guided引导模式
|
|
||||||
fc.mav_command(n, param); //起飞
|
|
||||||
}
|
|
||||||
if (n == 7) { //7 悬停
|
|
||||||
fc.mav_set_mode(5); //飞控设置成Loiter留待模式
|
|
||||||
}
|
|
||||||
if (n == 5) { //5 航点执行
|
|
||||||
fc.mav_set_mode(3); //飞控设置成auto自动模式
|
|
||||||
}
|
|
||||||
if (n == 8) { //8降落*
|
|
||||||
fc.mav_command(n, param);
|
|
||||||
}
|
|
||||||
if (n == 9) { //9返航
|
|
||||||
fc.mav_set_mode(6); //飞控设置成RTL返航
|
|
||||||
}
|
|
||||||
if (n == 11) { //11磁罗盘校准
|
|
||||||
fc.mav_command(n, param);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState
|
|
||||||
fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态
|
|
||||||
}else
|
|
||||||
if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState
|
|
||||||
topicPubMsg[10] = "1"; //恢复初始状态
|
|
||||||
}else
|
|
||||||
if (cutTopic == topicSub[4]) { //4:油门通道1
|
|
||||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
|
||||||
fc.channels[0] = strInt; //恢复初始状态
|
|
||||||
fc.mav_channels_override(fc.channels); //油门控制
|
|
||||||
}else
|
|
||||||
if (cutTopic == topicSub[5]) { //5:油门通道2
|
|
||||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
|
||||||
fc.channels[1] = strInt; //恢复初始状态
|
|
||||||
fc.mav_channels_override(fc.channels); //油门控制
|
|
||||||
}else
|
|
||||||
if (cutTopic == topicSub[6]) { //6:油门通道3
|
|
||||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
|
||||||
fc.channels[2] = strInt; //恢复初始状态
|
|
||||||
fc.mav_channels_override(fc.channels); //油门控制
|
|
||||||
}else
|
|
||||||
if (cutTopic == topicSub[7]) { //7:油门通道4
|
|
||||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
|
||||||
fc.channels[3] = strInt; //恢复初始状态
|
|
||||||
fc.mav_channels_override(fc.channels); //油门控制
|
|
||||||
}else
|
|
||||||
if (cutTopic == topicSub[8]) { //8:钩子控制
|
|
||||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //
|
|
||||||
ESP_LOGD(MOUDLENAME,"hookcontrol command: %d", strInt);
|
|
||||||
switch (strInt)
|
|
||||||
{
|
|
||||||
case 0: //收
|
|
||||||
/* code */
|
|
||||||
ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_UP");
|
|
||||||
if (motocontrol.getcontrolstatus().is_autogoodsdown)
|
|
||||||
up_action(SPEED_BTN_FAST);
|
|
||||||
break;
|
|
||||||
case 1: //放
|
|
||||||
/* code */
|
|
||||||
break;
|
|
||||||
case 2: //暂停
|
|
||||||
/* code */
|
|
||||||
ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_PAUSE");
|
|
||||||
if (motocontrol.getcontrolstatus().is_autogoodsdown)
|
|
||||||
Hook_stop();
|
|
||||||
break;
|
|
||||||
case 3: //继续
|
|
||||||
/* code */
|
|
||||||
ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_resume");
|
|
||||||
if (motocontrol.getcontrolstatus().is_autogoodsdown)
|
|
||||||
Hook_resume();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController
|
|
||||||
// json 反序列化
|
|
||||||
DynamicJsonDocument doc(0x2FFF);
|
|
||||||
deserializeJson(doc, topicStr);
|
|
||||||
JsonObject obj = doc.as<JsonObject>();
|
|
||||||
//相机控制
|
|
||||||
uint8_t item=obj["item"];
|
|
||||||
switch (item)
|
|
||||||
{
|
|
||||||
//一键回中
|
|
||||||
case 0:
|
|
||||||
fc.Camera_action_ret();
|
|
||||||
break;
|
|
||||||
//1:变焦
|
|
||||||
case 1:
|
|
||||||
fc.Camera_action_zoom(obj["val"]);
|
|
||||||
break;
|
|
||||||
//2:俯仰,旋转
|
|
||||||
case 2:
|
|
||||||
fc.Camera_action_move(obj["pitch"],obj["yaw"]);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
}else if (cutTopic == topicSub[10]) { //通用命令
|
|
||||||
// json 反序列化
|
|
||||||
DynamicJsonDocument doc(0x2FFF);
|
|
||||||
deserializeJson(doc, topicStr);
|
|
||||||
JsonObject obj = doc.as<JsonObject>();
|
|
||||||
uint8_t cmd_id=obj["id"];
|
|
||||||
ESP_LOGD(MOUDLENAME,"Mqtt_cmd: %d",cmd_id);
|
|
||||||
switch (cmd_id)
|
|
||||||
{
|
|
||||||
//校准称重
|
|
||||||
case 10:
|
|
||||||
if (motocontrol.gethooktatus() == HS_Stop)
|
|
||||||
begin_tare();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: 写入航点
|
|
||||||
* @param {String} topicStr mqtt订阅执行任务的Json字符串
|
|
||||||
*/
|
|
||||||
void writeRoute(String topicStr) {
|
|
||||||
if (fc.writeState) // 如果正在写入状态 跳出
|
|
||||||
{
|
|
||||||
ESP_LOGD(MOUDLENAME,"正在写航点"); // 提示正在写入中
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
//改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态
|
|
||||||
fc.pubMQTTmsg("planeState", topicPubMsg[10]); //发送正在写入的飞机状态
|
|
||||||
// json 反序列化
|
|
||||||
DynamicJsonDocument doc(0x2FFF);
|
|
||||||
deserializeJson(doc, topicStr);
|
|
||||||
JsonObject obj = doc.as<JsonObject>();
|
|
||||||
// 写入航点
|
|
||||||
uint8_t taskcount = obj["taskcount"]; //获取航点总数
|
|
||||||
fc.mav_mission_count(taskcount); //向飞控请求写入航点的数量
|
|
||||||
fc.writeState = true; //锁定写入状态
|
|
||||||
//监听飞控航点写入情况
|
|
||||||
while (true) {
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点
|
|
||||||
fc.comm_receive(mavlink_receiveCallback);
|
|
||||||
if (fc.missionArkType == 0) { //写入成功
|
|
||||||
fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态
|
|
||||||
ESP_LOGD(MOUDLENAME,"misson_bingo...");
|
|
||||||
//改变飞机状态
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态
|
|
||||||
break;
|
|
||||||
} else if (fc.missionArkType > 0) { //写入失败
|
|
||||||
fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态
|
|
||||||
ESP_LOGE(MOUDLENAME,"misson_error...");
|
|
||||||
//改变飞机状态
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
|
|
||||||
//当有成果反馈之后 初始化下列数据
|
|
||||||
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!=NULL)&&(str != "")&&(str != "null"))
|
|
||||||
{
|
|
||||||
fc.questVoiceStr = str;
|
|
||||||
//printf("writevoice str: %s \n", fc.questVoiceStr);
|
|
||||||
// if (fc.writeSeq==0)
|
|
||||||
//fc.playText(fc.questVoiceStr);
|
|
||||||
}
|
|
||||||
ESP_LOGD(MOUDLENAME,"frame--");
|
|
||||||
fc.logln(frame);
|
|
||||||
//写入航点
|
|
||||||
fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
|
|
||||||
fc.futureSeq++; //下一个航点序号
|
|
||||||
}
|
|
||||||
delay(200);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @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[10];
|
|
||||||
|
|
||||||
// printf("mav_id:%d\n",msg.msgid);
|
|
||||||
|
|
||||||
switch (msg.msgid) {
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
ESP_LOGD(MOUDLENAME,"armed");
|
|
||||||
fc.playText("已解锁");
|
|
||||||
}
|
|
||||||
curr_armed=true;
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
|
|
||||||
} else {
|
|
||||||
if (curr_armed)
|
|
||||||
{
|
|
||||||
ESP_LOGD(MOUDLENAME,"disarm");
|
|
||||||
fc.playText("已加锁");
|
|
||||||
}
|
|
||||||
curr_armed=false; //心跳里面 判断没有解锁
|
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为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] = "悬停";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 6:
|
|
||||||
{
|
|
||||||
topicPubMsg[12] = "返航";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 9:
|
|
||||||
{
|
|
||||||
topicPubMsg[12] = "降落";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 16:
|
|
||||||
{
|
|
||||||
topicPubMsg[12] = "定点";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
{
|
|
||||||
topicPubMsg[12] = "未知模式";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
fc.playText(topicPubMsg[12],true);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
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);
|
|
||||||
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_PARAM_VALUE: // #22: PARAM_VALUE
|
|
||||||
{
|
|
||||||
mavlink_param_value_t param_value;
|
|
||||||
mavlink_msg_param_value_decode(&msg, ¶m_value);
|
|
||||||
}
|
|
||||||
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.relative_alt / 1000);
|
|
||||||
|
|
||||||
|
|
||||||
if (topicPubMsg[4] != buf) {
|
|
||||||
topicPubMsg[4] = buf;
|
|
||||||
}
|
|
||||||
//海拔高度
|
|
||||||
sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}",
|
|
||||||
global_position_int.lon,
|
|
||||||
global_position_int.lat,
|
|
||||||
(double)global_position_int.alt / 1000
|
|
||||||
);
|
|
||||||
|
|
||||||
if (topicPubMsg[15] != buf) {
|
|
||||||
topicPubMsg[15] = buf;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
// 纬度
|
|
||||||
sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
|
|
||||||
if (topicPubMsg[7] != buf) {
|
|
||||||
topicPubMsg[7] = buf;
|
|
||||||
}
|
|
||||||
// 经度
|
|
||||||
sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
|
|
||||||
if (topicPubMsg[8] != buf) {
|
|
||||||
topicPubMsg[8] = buf;
|
|
||||||
}
|
|
||||||
// 卫星状态
|
|
||||||
switch (gps_raw_int.fix_type) {
|
|
||||||
case 0:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "No Fix";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 1:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "No Fix";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 2:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "Fix 2D";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 3:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "Fix 3D";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 4:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "Low GPS";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 5:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "Float";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
{
|
|
||||||
topicPubMsg[9] = "Fixed";
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈
|
|
||||||
{
|
|
||||||
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
|
|
||||||
mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
|
|
||||||
//日志
|
|
||||||
ESP_LOGD(MOUDLENAME,"MsgID:%d No:%d",msg.msgid,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数据
|
|
||||||
/*日志*/
|
|
||||||
ESP_LOGD(MOUDLENAME,"MsgID:%d re:",msg.msgid,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);
|
|
||||||
HookStatus hss=motocontrol.gethooktatus();
|
|
||||||
ESP_LOGD(MOUDLENAME,"COMMAND_LONG ID:%d,rng:%dcm",fc_hook_cmd,rngalt_cm);
|
|
||||||
//如果没初始化就退出
|
|
||||||
if (initstatus != IS_OK)
|
|
||||||
{
|
|
||||||
ESP_LOGE(MOUDLENAME,"not initstatus:%d",initstatus);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
switch (fc_hook_cmd)
|
|
||||||
{
|
|
||||||
//自动放线
|
|
||||||
case MAV_CMD_FC_HOOK_AUTODOWN:
|
|
||||||
{
|
|
||||||
// HookStatus hss=motocontrol.gethooktatus();
|
|
||||||
ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm",hss,rngalt_cm);
|
|
||||||
//没有激光高度直接退出
|
|
||||||
if (rngalt_cm==0)
|
|
||||||
{
|
|
||||||
ESP_LOGE(MOUDLENAME,"exit rngalt_cm==0");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (hss==HS_Locked)
|
|
||||||
{
|
|
||||||
Hook_autodown(rngalt_cm);
|
|
||||||
//最大音量语音播报1次
|
|
||||||
if (fc.questVoiceStr!="")
|
|
||||||
fc.playText(fc.questVoiceStr,true);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if (hss==HS_Stop)
|
|
||||||
Hook_resume();
|
|
||||||
else
|
|
||||||
ESP_LOGE(MOUDLENAME,"exit hooktatus error");
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
//暂停
|
|
||||||
case MAV_CMD_FC_HOOK_PAUSE:
|
|
||||||
{
|
|
||||||
ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_PAUSE");
|
|
||||||
Hook_stop();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
//暂停
|
|
||||||
case MAV_CMD_FC_HOOK_RECOVERY:
|
|
||||||
{
|
|
||||||
ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_RECOVERY ");
|
|
||||||
Hook_recovery();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
//开始下降
|
|
||||||
case MAV_CMD_FC_DESCENTSTART:
|
|
||||||
{
|
|
||||||
ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_DESCENTSTART ");
|
|
||||||
fc.Camera_action_down();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
//当前执行到的任务号
|
|
||||||
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
|
|
||||||
{
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: 发送主题线程
|
|
||||||
*/
|
|
||||||
void pubThread() {
|
|
||||||
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
|
|
||||||
// 创建一个JSON对象
|
|
||||||
DynamicJsonDocument doc(2000); // 2000字节 缓冲区
|
|
||||||
//遍历 有更新的数据 组成一个json对象
|
|
||||||
for (int i = 0; i < topicPubCount; i++) {
|
|
||||||
if (topicPubMsg[i] != oldMsg[i]) {
|
|
||||||
if (i == 0) { //心跳包 每每向心跳主题发布信息
|
|
||||||
//启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
|
|
||||||
if (fc.getIsInit()) {
|
|
||||||
fc.setIsInit(false);
|
|
||||||
fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容
|
|
||||||
}
|
|
||||||
//设置对象成员 ps:心跳
|
|
||||||
doc[topicPub[0]] = topicPubMsg[0];
|
|
||||||
} else { //非心跳 有更新 录入成员
|
|
||||||
doc[topicPub[i]] = topicPubMsg[i];
|
|
||||||
oldMsg[i] = topicPubMsg[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// 将JSON对象序列化为JSON字符串
|
|
||||||
String jsonString;
|
|
||||||
serializeJson(doc, jsonString);
|
|
||||||
fc.pubMQTTmsg("planeState", jsonString);
|
|
||||||
/*更新4G网络测速ping值*/
|
|
||||||
//pingNetTest();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
|
|
||||||
*/
|
|
||||||
void flashThread() {
|
|
||||||
if (digitalRead(23) == HIGH) {
|
|
||||||
if (isPush) { //点击之后
|
|
||||||
//请求注册 ps:发送esp8266的物理地址 到对频主题
|
|
||||||
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
|
|
||||||
}
|
|
||||||
isPush = false; //复位按钮
|
|
||||||
} else {
|
|
||||||
//FLASH按下状态
|
|
||||||
isPush = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
|
||||||
*/
|
|
||||||
void mavThread() {
|
|
||||||
fc.mav_request_data();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @description: 向飞控 发送油门指令
|
|
||||||
*/
|
|
||||||
void chanThread() {
|
|
||||||
//mav_channels_override(channels);
|
|
||||||
}
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user