【类 型】：feat
【原 因】：飞控mavlink发送 ADSB数据 【过 程】： 【影 响】： # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
This commit is contained in:
air
parent
43dfbc91b2
commit
212a9e5da1
@ -5,7 +5,7 @@
|
||||
//char* password = "63587839ab"; //wifi密码
|
||||
char* ssid = "flicube"; //wifi帐号
|
||||
char* password = "fxmf0622"; //wifi密码
|
||||
char* mqttServer = "szdot.top"; //mqtt地址
|
||||
char* mqttServer = "wxsky.com"; //mqtt地址
|
||||
int mqttPort = 1883; //mqtt端口
|
||||
char* mqttName = "admin"; //mqtt帐号
|
||||
char* mqttPassword = "123456"; //mqtt密码
|
||||
|
||||
@ -1,118 +1,136 @@
|
||||
#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) {
|
||||
* @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; //云台相机端口
|
||||
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 (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;
|
||||
// 初始化声音模块串口 波特率
|
||||
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) {
|
||||
* @description: 日志打印
|
||||
* @param {*} val 输出的信息
|
||||
*/
|
||||
void FoodCube::log(String val)
|
||||
{
|
||||
Serial.print(val);
|
||||
}
|
||||
void FoodCube::log(char* val) {
|
||||
void FoodCube::log(char *val)
|
||||
{
|
||||
Serial.print(val);
|
||||
}
|
||||
void FoodCube::log(int val) {
|
||||
void FoodCube::log(int val)
|
||||
{
|
||||
Serial.print(val);
|
||||
}
|
||||
void FoodCube::log(IPAddress val) {
|
||||
void FoodCube::log(IPAddress val)
|
||||
{
|
||||
Serial.print(val);
|
||||
}
|
||||
void FoodCube::log(bool val) {
|
||||
void FoodCube::log(bool val)
|
||||
{
|
||||
Serial.print(val);
|
||||
}
|
||||
void FoodCube::logln(String val) {
|
||||
void FoodCube::logln(String val)
|
||||
{
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(char* val) {
|
||||
void FoodCube::logln(char *val)
|
||||
{
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(int val) {
|
||||
void FoodCube::logln(int val)
|
||||
{
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(IPAddress val) {
|
||||
void FoodCube::logln(IPAddress val)
|
||||
{
|
||||
Serial.println(val);
|
||||
}
|
||||
void FoodCube::logln(bool val) {
|
||||
void FoodCube::logln(bool val)
|
||||
{
|
||||
Serial.print(val);
|
||||
}
|
||||
|
||||
/**
|
||||
*@description: 取值 设置值
|
||||
*/
|
||||
bool FoodCube::getIsInit() {
|
||||
*@description: 取值 设置值
|
||||
*/
|
||||
bool FoodCube::getIsInit()
|
||||
{
|
||||
return isInit;
|
||||
}
|
||||
void FoodCube::setIsInit(bool b) {
|
||||
void FoodCube::setIsInit(bool b)
|
||||
{
|
||||
isInit = b;
|
||||
}
|
||||
String FoodCube::getMacAdd() {
|
||||
String FoodCube::getMacAdd()
|
||||
{
|
||||
return macAdd;
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 连接wifi
|
||||
*/
|
||||
void FoodCube::connectWifi() {
|
||||
//设置wifi帐号密码
|
||||
* @description: 连接wifi
|
||||
*/
|
||||
void FoodCube::connectWifi()
|
||||
{
|
||||
// 设置wifi帐号密码
|
||||
WiFi.begin(ssid, password);
|
||||
//连接wifi
|
||||
// 连接wifi
|
||||
logln("Connecting Wifi...");
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
while (WiFi.status() != WL_CONNECTED)
|
||||
{
|
||||
log(".");
|
||||
delay(150);
|
||||
}
|
||||
//获取局域网ip
|
||||
// 获取局域网ip
|
||||
logln("");
|
||||
logln("WiFi connected");
|
||||
log("IP address: ");
|
||||
logln(WiFi.localIP());
|
||||
localIp = WiFi.localIP();
|
||||
//设置开发板为无线终端 获取物理mac地址
|
||||
// 设置开发板为无线终端 获取物理mac地址
|
||||
WiFi.mode(WIFI_STA);
|
||||
macAdd = WiFi.macAddress();
|
||||
macAdd.replace(":", ""); //板子的物理地址 并且去除冒号
|
||||
macAdd.replace(":", ""); // 板子的物理地址 并且去除冒号
|
||||
log("macAdd: ");
|
||||
logln(macAdd);
|
||||
playText("歪佛哎,已连接");
|
||||
@ -120,23 +138,27 @@ void FoodCube::connectWifi() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 连接mqtt
|
||||
* @param {String} topicSub 订阅主题
|
||||
*/
|
||||
void FoodCube::connectMqtt(String topicSub) {
|
||||
* @description: 连接mqtt
|
||||
* @param {String} topicSub 订阅主题
|
||||
*/
|
||||
void FoodCube::connectMqtt(String topicSub)
|
||||
{
|
||||
/*尝试连接mqtt*/
|
||||
if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) {
|
||||
if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword))
|
||||
{
|
||||
logln("MQTT Server Connected.");
|
||||
log("Server Address: ");
|
||||
logln(mqttServer);
|
||||
log("ClientId :");
|
||||
logln(macAdd);
|
||||
/*连接成功 订阅主题*/
|
||||
//订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback
|
||||
// 订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback
|
||||
subscribeTopic(topicSub, 1);
|
||||
playText("服务器,已连接");
|
||||
} else {
|
||||
//失败返回状态码
|
||||
}
|
||||
else
|
||||
{
|
||||
// 失败返回状态码
|
||||
log("MQTT Server Connect Failed. Client State:");
|
||||
logln(mqttClient->state());
|
||||
delay(3000);
|
||||
@ -144,87 +166,95 @@ void FoodCube::connectMqtt(String topicSub) {
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
|
||||
* @param {String} topicSub 订阅主题
|
||||
*/
|
||||
void FoodCube::mqttLoop(String topicSub) {
|
||||
if (mqttClient->connected()) { //检测 如果开发板成功连接服务器
|
||||
mqttClient->loop(); // 保持心跳
|
||||
} else { // 如果开发板未能成功连接服务器
|
||||
connectMqtt(topicSub); // 则尝试连接服务器
|
||||
* @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
|
||||
* @param {String} topicSub 订阅主题
|
||||
*/
|
||||
void FoodCube::mqttLoop(String topicSub)
|
||||
{
|
||||
if (mqttClient->connected())
|
||||
{ // 检测 如果开发板成功连接服务器
|
||||
mqttClient->loop(); // 保持心跳
|
||||
}
|
||||
else
|
||||
{ // 如果开发板未能成功连接服务器
|
||||
connectMqtt(topicSub); // 则尝试连接服务器
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 订阅指定主题
|
||||
* @param {String} topicString 主题名称
|
||||
* @param {int} Qos 1：重要 ps:响应时间快
|
||||
*/
|
||||
void FoodCube::subscribeTopic(String topicString, int Qos = 1) {
|
||||
//处理主题字符串
|
||||
* @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) {
|
||||
//处理主题字符串
|
||||
* @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 {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) {
|
||||
* @description: 声音播放
|
||||
* @param {String} str 播放内容
|
||||
*/
|
||||
void FoodCube::playText(String str)
|
||||
{
|
||||
/*消息长度*/
|
||||
int len = str.length(); //修改信息长度 消息长度
|
||||
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
|
||||
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编码 先给值
|
||||
// 已知 帧头0xFD z信息长度高位 x信息长度低位 0x01命令字1是开始命令 0x04utf8编码 先给值
|
||||
int index = 0;
|
||||
command[index++] = 0xFD;
|
||||
command[index++] = highByte;
|
||||
@ -232,71 +262,81 @@ void FoodCube::playText(String str) {
|
||||
command[index++] = 0x01;
|
||||
command[index++] = 0x04;
|
||||
/*帧命令 消息段*/
|
||||
for (int i = 0; i < str.length(); i++) {
|
||||
for (int i = 0; i < str.length(); i++)
|
||||
{
|
||||
command[index++] = (int)str[i];
|
||||
}
|
||||
for (int i = 0; i < sizeof(command); 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() {
|
||||
* @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;
|
||||
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: 声音模块停止合成
|
||||
*/
|
||||
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) {
|
||||
* @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) {
|
||||
while (len-- != 0)
|
||||
{
|
||||
temp = (crc >> 8) & 0xff;
|
||||
oldcrc16 = crc16_tab[*ptr ^ temp];
|
||||
crc = (crc << 8) ^ oldcrc16;
|
||||
@ -305,58 +345,64 @@ uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) {
|
||||
return (crc);
|
||||
}
|
||||
|
||||
void FoodCube::crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result) {
|
||||
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) {
|
||||
* @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;
|
||||
//计算校检码
|
||||
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++) {
|
||||
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发送命令帧
|
||||
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) {
|
||||
* @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
|
||||
if (i)
|
||||
{
|
||||
val |= (1u << n); // 按位设置成1
|
||||
}
|
||||
else
|
||||
{
|
||||
val &= ~(1u << n); // 按位设置成0
|
||||
}
|
||||
sprintf(buf, "%d", val);
|
||||
return buf;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @description: 向飞控请求 指定数据
|
||||
*/
|
||||
void FoodCube::mav_request_data() {
|
||||
void FoodCube::mav_request_data()
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
// 以下注释 把数据流组合到一起
|
||||
@ -373,10 +419,11 @@ void FoodCube::mav_request_data() {
|
||||
* 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++) {
|
||||
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);
|
||||
@ -388,26 +435,31 @@ void FoodCube::mav_request_data() {
|
||||
* @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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -415,13 +467,14 @@ void FoodCube::comm_receive(void (*pFun)(uint8_t)) {
|
||||
* @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]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
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);
|
||||
}
|
||||
|
||||
@ -431,7 +484,7 @@ void FoodCube::mav_mission_count(uint8_t taskcount) {
|
||||
* @param {uint8_t} frame 通常第一个是home航点0 绝对海拔 后面的固定的全是 3 相对高度
|
||||
* @param {uint8_t} command 航点的类型 如：16 是waypoint模式
|
||||
* @param {uint8_t} current 固定给0
|
||||
* @param {uint8_t} autocontinue 是否自动飞下一个航点
|
||||
* @param {uint8_t} autocontinue 是否自动飞下一个航点
|
||||
* @param {double} param1
|
||||
* @param {double} param2
|
||||
* @param {double} param3
|
||||
@ -440,48 +493,53 @@ void FoodCube::mav_mission_count(uint8_t taskcount) {
|
||||
* @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]; //发送的缓存
|
||||
//写入航点数据包
|
||||
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]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
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[]
|
||||
* @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]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
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) { //飞机加解锁
|
||||
if (controlType == 3)
|
||||
{ // 飞机加解锁
|
||||
cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
|
||||
cmd.param1 = param[0]; // 0:加锁 1：解锁
|
||||
cmd.param2 = param[1]; //0:符合条件加解锁 21196:强制加解锁
|
||||
cmd.param1 = param[0]; // 0:加锁 1：解锁
|
||||
cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁
|
||||
}
|
||||
if (controlType == 6) { //测试起飞
|
||||
if (controlType == 6)
|
||||
{ // 测试起飞
|
||||
float p = (float)param[0];
|
||||
cmd.command = MAV_CMD_NAV_TAKEOFF;
|
||||
cmd.param1 = 0;
|
||||
@ -490,12 +548,14 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
|
||||
cmd.param4 = 0;
|
||||
cmd.param5 = 0;
|
||||
cmd.param6 = 0;
|
||||
cmd.param7 = p; //起飞高度
|
||||
cmd.param7 = p; // 起飞高度
|
||||
}
|
||||
if (controlType == 8) { //降落
|
||||
if (controlType == 8)
|
||||
{ // 降落
|
||||
cmd.command = MAV_CMD_NAV_LAND;
|
||||
}
|
||||
if (controlType == 11) { //磁罗盘校准
|
||||
if (controlType == 11)
|
||||
{ // 磁罗盘校准
|
||||
cmd.command = MAV_CMD_DO_START_MAG_CAL;
|
||||
cmd.param1 = 0;
|
||||
cmd.param2 = 1;
|
||||
@ -503,19 +563,20 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
|
||||
}
|
||||
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]; //发送的缓存
|
||||
//控制油门
|
||||
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);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user