原始版
This commit is contained in:
commit
d2c4d566fd
546
FoodDelivery.ino
Normal file
546
FoodDelivery.ino
Normal file
@ -0,0 +1,546 @@
|
||||
#include "FoodDeliveryBase.h"
|
||||
|
||||
/*项目对象*/
|
||||
//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); //创建项目对象
|
||||
/*订阅 主题*/
|
||||
//0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 8:钩子控制 9:云台相机控制
|
||||
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4", "hookConteroller", "cameraController" }; //订阅主题
|
||||
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数
|
||||
/*有更新主动发送 主题*/
|
||||
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式
|
||||
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode" };
|
||||
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数
|
||||
String topicPubMsg[13]; //发送数据存放 对应topicPub
|
||||
String oldMsg[13]; //记录旧的数据 用来对比有没有更新
|
||||
/*触发发送 主题*/
|
||||
//0:对频信息
|
||||
String topicHandle[] = { "crosFrequency" };
|
||||
boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微动开关
|
||||
|
||||
/*异步线程对象*/
|
||||
Ticker pubTicker; //定时发布主题 线程
|
||||
Ticker mavTicker; //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||
Ticker flashTicker; //单片机主动 按钮主动发布主题 线程
|
||||
Ticker chanTicker; //定时向飞控 发送油门指定
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200); //日志串口
|
||||
Serial1.begin(115200, SERIAL_8N1, 18, 5); //声音模块引串口脚映射
|
||||
/*初始化*/
|
||||
fc.connectWifi(); //连接wifi
|
||||
fc.connectMqtt(topicSub, topicSubCount); //连接mqtt
|
||||
fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调
|
||||
fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
|
||||
|
||||
/*异步线程*/
|
||||
pubTicker.attach(1, pubThread); //定时 发布主题
|
||||
mavTicker.attach(10, mavThread); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||
flashTicker.attach_ms(50, flashThread); //监听 按flash键时 主动发布对频主题
|
||||
|
||||
pinMode(23, INPUT_PULLUP); // 记得删除
|
||||
}
|
||||
|
||||
void loop() {
|
||||
/*从飞控拿数据*/
|
||||
fc.comm_receive(mavlink_receiveCallback);
|
||||
/*保持mqtt心跳*/
|
||||
fc.mqttLoop(topicSub, topicSubCount);
|
||||
delay(50);
|
||||
}
|
||||
|
||||
/**
|
||||
* @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); //起飞
|
||||
} 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 (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:钩子控制 hookConteroller
|
||||
//topicStr 0:收 1:放 2:暂停 3:继续
|
||||
} else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController
|
||||
// json 反序列化
|
||||
DynamicJsonDocument doc(0x2FFF);
|
||||
deserializeJson(doc, topicStr);
|
||||
JsonObject obj = doc.as<JsonObject>();
|
||||
//相机控制
|
||||
if (obj["item"] = "0") { //0:一键回中
|
||||
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
|
||||
size_t len = sizeof(command);
|
||||
} else if (obj["item"] = "1") { //1:变焦
|
||||
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x0 };
|
||||
command[8] = (uint16_t)obj["val"].toInt();
|
||||
size_t len = sizeof(command);
|
||||
} else if (obj["item"] = "2") { //2:俯仰
|
||||
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
|
||||
command[9] = (uint16_t)obj["val"].toInt();
|
||||
size_t len = sizeof(command);
|
||||
}else if (obj["item"] = "3") { //2:旋转
|
||||
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
|
||||
command[8] = (uint16_t)obj["val"].toInt();
|
||||
size_t len = sizeof(command);
|
||||
}
|
||||
fc.udpSendToCamera(command, len);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 写入航点
|
||||
* @param {String} topicStr mqtt订阅执行任务的Json字符串
|
||||
*/
|
||||
void writeRoute(String topicStr) {
|
||||
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", 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默认状态
|
||||
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"];
|
||||
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(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) {
|
||||
// 尝试从数据流里 解析数据
|
||||
if (mavlink_parse_char(MAVLINK_COMM_0, c, pMsg, pStatus)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
|
||||
// 通过msgid来判断 数据流的类别
|
||||
char buf[10];
|
||||
switch (pMsg->msgid) {
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
|
||||
{
|
||||
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
|
||||
mavlink_msg_heartbeat_decode(pMsg, &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(pMsg, &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(pMsg, ¶m_value);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU
|
||||
{
|
||||
mavlink_raw_imu_t raw_imu;
|
||||
mavlink_msg_raw_imu_decode(pMsg, &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(pMsg, &global_position_int);
|
||||
// 高度
|
||||
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
|
||||
if (topicPubMsg[4] != buf) {
|
||||
topicPubMsg[4] = buf;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘
|
||||
{
|
||||
mavlink_vfr_hud_t vfr_hud;
|
||||
mavlink_msg_vfr_hud_decode(pMsg, &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(pMsg, &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(pMsg, &mission_request); // 解构msg数据
|
||||
//日志
|
||||
fc.logln("MsgID:");
|
||||
fc.logln(pMsg->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(pMsg, &mission_ark); // 解构msg数据
|
||||
/*日志*/
|
||||
fc.logln("MsgID:");
|
||||
fc.logln(pMsg->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 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/
|
||||
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(); //再向飞控请求一次 设定飞控输出数据流内容
|
||||
}
|
||||
//发送心跳包
|
||||
fc.pubMQTTmsg(topicPub[0], topicPubMsg[0]);
|
||||
} else { //非心跳包 有更新才向对应主题发布信息
|
||||
fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]);
|
||||
oldMsg[i] = topicPubMsg[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
/*更新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);
|
||||
}
|
||||
525
FoodDeliveryBase.cpp
Normal file
525
FoodDeliveryBase.cpp
Normal file
@ -0,0 +1,525 @@
|
||||
#include "FoodDeliveryBase.h"
|
||||
|
||||
/**
|
||||
* @description: 初始化
|
||||
*/
|
||||
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; //云台相机端口
|
||||
|
||||
//初始化飞控通讯串口 波特率
|
||||
switch (mavlinkSerial) { //初始化指定 串口号
|
||||
case 0:
|
||||
Serial.begin(57600);
|
||||
break;
|
||||
case 1:
|
||||
Serial1.begin(57600);
|
||||
break;
|
||||
case 2:
|
||||
Serial2.begin(57600);
|
||||
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);
|
||||
}
|
||||
/**
|
||||
* @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) {
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(char* val) {
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(int val) {
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(IPAddress val) {
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(bool val) {
|
||||
Serial.print(val);
|
||||
}
|
||||
|
||||
/**
|
||||
*@description: 取值 设置值
|
||||
*/
|
||||
bool FoodCube::getIsInit() {
|
||||
return isInit;
|
||||
}
|
||||
void FoodCube::setIsInit(bool b) {
|
||||
isInit = b;
|
||||
}
|
||||
String FoodCube::getMacAdd() {
|
||||
return macAdd;
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 连接wifi
|
||||
*/
|
||||
void FoodCube::connectWifi() {
|
||||
//设置wifi帐号密码
|
||||
WiFi.begin(ssid, password);
|
||||
//连接wifi
|
||||
logln("Connecting Wifi...");
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
log(".");
|
||||
delay(150);
|
||||
}
|
||||
//获取局域网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("歪佛哎,已连接");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 连接mqtt
|
||||
* @param {String[]} topicSub 主题数组
|
||||
* @param {int} topicSubCount 数组总数
|
||||
*/
|
||||
void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
|
||||
/*尝试连接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
|
||||
for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题
|
||||
subscribeTopic(topicSub[i], 1);
|
||||
}
|
||||
playText("哎木可优踢踢,已连接");
|
||||
} else {
|
||||
//失败返回状态码
|
||||
log("MQTT Server Connect Failed. Client State:");
|
||||
logln(mqttClient->state());
|
||||
delay(3000);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
|
||||
* @param {String[]} topicSub 主题数组
|
||||
* @param {int} topicSubCount 数组总数
|
||||
*/
|
||||
void FoodCube::mqttLoop(String topicSub[], int topicSubCount) {
|
||||
if (mqttClient->connected()) { //检测 如果开发板成功连接服务器
|
||||
mqttClient->loop(); // 保持心跳
|
||||
} else { // 如果开发板未能成功连接服务器
|
||||
connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @description: 声音播放
|
||||
* @param {String} str 播放内容
|
||||
*/
|
||||
void FoodCube::playText(String str) {
|
||||
/*消息长度*/
|
||||
int len = str.length(); //修改信息长度 消息长度
|
||||
// if (len >= 3996) { //限制信息长度 超长的不播放
|
||||
// return;
|
||||
// }
|
||||
/*长度高低位*/
|
||||
int frameLength = len + 2; // 5 bytes for header and length bytes
|
||||
byte highByte = (frameLength >> 8) & 0xFF; // extract high byte of length
|
||||
byte lowByte = frameLength & 0xFF; // extract low byte of length
|
||||
/*帧命令头*/
|
||||
uint8_t command[len + 5];
|
||||
//已知 帧头0xFD z信息长度高位 x信息长度低位 0x01命令字1是开始命令 0x04utf8编码 先给值
|
||||
int index = 0;
|
||||
command[index++] = 0xFD;
|
||||
command[index++] = highByte;
|
||||
command[index++] = lowByte;
|
||||
command[index++] = 0x01;
|
||||
command[index++] = 0x04;
|
||||
/*帧命令 消息段*/
|
||||
for (int i = 0; i < str.length(); i++) {
|
||||
command[index++] = (int)str[i];
|
||||
}
|
||||
for (int i = 0; i < sizeof(command); i++) {
|
||||
log(command[i]);
|
||||
log(" ");
|
||||
}
|
||||
logln("");
|
||||
//串口发送 播放声音
|
||||
SWrite(command, sizeof(command), voiceSerial);
|
||||
}
|
||||
|
||||
/**
|
||||
* @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: 相机
|
||||
* @param {uint8_t[]} 命令帧 数组
|
||||
* @param {int} len 命令帧 的长度
|
||||
*/
|
||||
void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) {
|
||||
uint16_t result = 0;
|
||||
uint16_t* p_result = &result;
|
||||
//计算校检码
|
||||
crc_check_16bites(p_command, len, p_result);
|
||||
//加上校检码
|
||||
uint8_t bytes[2 + len];
|
||||
for (int i = 0; i < len; i++) {
|
||||
bytes[i] = p_command[i];
|
||||
}
|
||||
bytes[len] = static_cast<uint8_t>(result); // 低位 互换
|
||||
bytes[len + 1] = static_cast<uint8_t>(result >> 8); // 高位 互换
|
||||
//udp发送命令帧
|
||||
udp.beginPacket(udpServerIP, udpServerPort);
|
||||
udp.write(bytes, len + 2);
|
||||
udp.endPacket();
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 按位设置 1 or 0
|
||||
* @param {String} str 要设置的值
|
||||
* @param {uint8_t} n 第几位 从0开始
|
||||
* @param {uint8_t} isOne 1:设置成1 0:设置成0
|
||||
*/
|
||||
String FoodCube::setNBit(String str, uint8_t n, uint8_t i) {
|
||||
char buf[10];
|
||||
uint16_t val = (uint8_t)str.toInt();
|
||||
if (i) {
|
||||
val |= (1u << n); //按位设置成1
|
||||
} else {
|
||||
val &= ~(1u << n); //按位设置成0
|
||||
}
|
||||
sprintf(buf, "%d", val);
|
||||
return buf;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @description: 向飞控请求 指定数据
|
||||
*/
|
||||
void FoodCube::mav_request_data() {
|
||||
mavlink_message_t msg;
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
// 以下注释 把数据流组合到一起
|
||||
/*
|
||||
* MAV_DATA_STREAM_ALL=0, // 获取所有类别 数据流
|
||||
* MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
|
||||
* MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
|
||||
* MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
|
||||
* MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.
|
||||
* MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
|
||||
* MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot
|
||||
* MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot
|
||||
* MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot
|
||||
* MAV_DATA_STREAM_ENUM_END=13,
|
||||
*/
|
||||
// 根据从Pixhawk请求的所需信息进行设置
|
||||
const int maxStreams = 1; // 遍历次数 （下面组合的长度）
|
||||
const uint8_t MAVStreams[maxStreams] = { MAV_DATA_STREAM_ALL }; // 请求的数据流组合 放到一个对象 后面进行遍历
|
||||
const uint16_t MAVRates[maxStreams] = { 0x01 }; // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹 既每秒发送一次
|
||||
for (int i = 0; i < maxStreams; i++) {
|
||||
// 向飞控发送请求
|
||||
mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 解析mavlink数据流
|
||||
* @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
|
||||
*/
|
||||
void FoodCube::comm_receive(void (*pFun)(uint8_t)) {
|
||||
switch (mavlinkSerial) {
|
||||
case 0:
|
||||
while (Serial.available() > 0) { // 判断串口缓存 是否有数据
|
||||
uint8_t c = Serial.read(); // 从缓存拿到数据
|
||||
pFun(c);
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
while (Serial1.available() > 0) { // 判断串口缓存 是否有数据
|
||||
uint8_t c = Serial1.read(); // 从缓存拿到数据
|
||||
pFun(c);
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
while (Serial2.available() > 0) { // 判断串口缓存 是否有数据
|
||||
uint8_t c = Serial2.read(); // 从缓存拿到数据
|
||||
pFun(c);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 写入航点 第一步 “向飞控发送航点数量”
|
||||
* @param {uint8_t} taskcount 航点数量
|
||||
*/
|
||||
void FoodCube::mav_mission_count(uint8_t taskcount) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
mavlink_msg_mission_count_pack(0xFF, 0xBE, &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(0xFF, 0xBE, &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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 向飞控发送 设置模式指令
|
||||
*/
|
||||
void FoodCube::mav_set_mode(uint8_t SysState) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
mavlink_msg_set_mode_pack(0xFF, 0xBE, &msg, 1, 209, SysState);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 飞控 控制
|
||||
* @param {uint8_t} controlType 3:加解锁模式控制
|
||||
* @param {uint16_t} param[]
|
||||
*/
|
||||
void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
mavlink_command_long_t cmd;
|
||||
cmd.target_system = 1;
|
||||
cmd.target_component = 1;
|
||||
cmd.confirmation = 0;
|
||||
if (controlType == 3) { //飞机加解锁
|
||||
cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
|
||||
cmd.param1 = param[0]; // 0:加锁 1：解锁
|
||||
cmd.param2 = param[1]; //0:符合条件加解锁 21196:强制加解锁
|
||||
}
|
||||
if (controlType == 6) { //测试起飞
|
||||
float p = (float)param[0];
|
||||
cmd.command = MAV_CMD_NAV_TAKEOFF;
|
||||
cmd.param1 = 0;
|
||||
cmd.param2 = 0;
|
||||
cmd.param3 = 0;
|
||||
cmd.param4 = 0;
|
||||
cmd.param5 = 0;
|
||||
cmd.param6 = 0;
|
||||
cmd.param7 = p; //起飞高度
|
||||
}
|
||||
if (controlType == 8) { //降落
|
||||
cmd.command = MAV_CMD_NAV_LAND;
|
||||
}
|
||||
if (controlType == 11) { //磁罗盘校准
|
||||
cmd.command = MAV_CMD_DO_START_MAG_CAL;
|
||||
cmd.param1 = 0;
|
||||
cmd.param2 = 1;
|
||||
cmd.param3 = 1;
|
||||
}
|
||||
mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &cmd);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
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);
|
||||
}
|
||||
126
FoodDeliveryBase.h
Normal file
126
FoodDeliveryBase.h
Normal file
@ -0,0 +1,126 @@
|
||||
#ifndef _FOODDELIVERYBASE_H_
|
||||
#define _FOODDELIVERYBASE_H_
|
||||
#include "Arduino.h"
|
||||
/*wifi*/
|
||||
#include "WiFi.h"
|
||||
/*mqtt*/
|
||||
#include "PubSubClient.h"
|
||||
/*mavlink*/
|
||||
#include "mavlink.h"
|
||||
/*json库*/
|
||||
#include "ArduinoJson.h"
|
||||
/*异步库*/
|
||||
#include "Ticker.h"
|
||||
/*udp发送*/
|
||||
#include "WiFiUdp.h"
|
||||
|
||||
class FoodCube {
|
||||
public:
|
||||
/*飞行航点任务相关属性*/
|
||||
bool writeState = false; //是否是写入状态
|
||||
int8_t writeSeq = -1; //飞控反馈 需写入航点序列号
|
||||
int8_t futureSeq = 0; //记录将来要写入 航点序列号
|
||||
int8_t missionArkType = -1; //航点写入是否成功
|
||||
/*前端模拟遥控的油门通道*/
|
||||
uint16_t channels[4] = { 1500, 1500, 1500, 1500 };
|
||||
/*初始化*/
|
||||
FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial,char* userUdpServerIP, uint32_t userUdpServerPort);
|
||||
/*日志打印*/
|
||||
void log(String val);
|
||||
void log(char* val);
|
||||
void log(int val);
|
||||
void log(IPAddress val);
|
||||
void log(bool val);
|
||||
void logln(String val);
|
||||
void logln(char* val);
|
||||
void logln(int val);
|
||||
void logln(IPAddress val);
|
||||
void logln(bool val);
|
||||
/*get set value*/
|
||||
bool getIsInit();
|
||||
void setIsInit(bool b);
|
||||
String getMacAdd();
|
||||
/*wifi*/
|
||||
void connectWifi();
|
||||
/*mqtt*/
|
||||
PubSubClient* mqttClient; //指向 mqtt服务器连接 对象
|
||||
void connectMqtt(String topicSub[], int topicSubCount);
|
||||
void mqttLoop(String topicSub[], int topicSubCount);
|
||||
void subscribeTopic(String topicString, int Qos);
|
||||
void pubMQTTmsg(String topicString, String messageString);
|
||||
/*串口输出*/
|
||||
void SWrite(uint8_t buf[], int len, uint8_t swSerial);
|
||||
/*声音模块控制*/
|
||||
void playText(String str);
|
||||
uint8_t chekVoiceMcu();
|
||||
void stopVoice();
|
||||
/*mavlink*/
|
||||
String setNBit(String str, uint8_t n, uint8_t i);
|
||||
void mav_request_data();
|
||||
void comm_receive(void (*pFun)(uint8_t));
|
||||
void mav_mission_count(uint8_t taskcount);
|
||||
void 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);
|
||||
void mav_set_mode(uint8_t SysState);
|
||||
void mav_command(uint8_t controlType, uint16_t param[]);
|
||||
void mav_channels_override(uint16_t chan[]);
|
||||
/*云台相机控制*/
|
||||
void udpSendToCamera(uint8_t* p_command, uint32_t len);
|
||||
|
||||
private:
|
||||
char* ssid; //wifi帐号
|
||||
char* password; //wifi密码
|
||||
char* mqttServer; //mqtt服务器地址
|
||||
int mqttPort; //mqtt服务器端口
|
||||
char* mqttName; //mqtt帐号
|
||||
char* mqttPassword; //mqtt密码
|
||||
uint8_t mavlinkSerial; //飞控占用的串口号
|
||||
uint8_t voiceSerial; //飞控占用的串口号
|
||||
bool isInit = true; //用来判断接收到第一次心跳时 重新向飞控 发送一个请求数据类型
|
||||
|
||||
WiFiClient wifiClient; //网络客户端
|
||||
IPAddress localIp; //板子的IP地址
|
||||
String macAdd; //板子的物理地址(已去掉冒号分隔符)
|
||||
|
||||
/*云台相机控制*/
|
||||
WiFiUDP udp; //udp信息操作对象
|
||||
char* udpServerIP; //云台相机ip地址
|
||||
uint32_t udpServerPort; //云台相机端口
|
||||
//摄像头控制 校验代码
|
||||
const uint16_t crc16_tab[256] = { 0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
|
||||
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
|
||||
0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
|
||||
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
|
||||
0x2462, 0x3443, 0x420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
|
||||
0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
|
||||
0x3653, 0x2672, 0x1611, 0x630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
|
||||
0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
|
||||
0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840, 0x1861, 0x2802, 0x3823,
|
||||
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
|
||||
0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50, 0x3a33, 0x2a12,
|
||||
0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
|
||||
0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0xc60, 0x1c41,
|
||||
0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
|
||||
0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0xe70,
|
||||
0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
|
||||
0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
|
||||
0x1080, 0xa1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
|
||||
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
|
||||
0x2b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
|
||||
0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
|
||||
0x34e2, 0x24c3, 0x14a0, 0x481, 0x7466, 0x6447, 0x5424, 0x4405,
|
||||
0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
|
||||
0x26d3, 0x36f2, 0x691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
|
||||
0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
|
||||
0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1, 0x3882, 0x28a3,
|
||||
0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
|
||||
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1, 0x1ad0, 0x2ab3, 0x3a92,
|
||||
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
|
||||
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,
|
||||
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
|
||||
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0 };
|
||||
uint16_t CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init);
|
||||
void crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user