Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

【类 型】：style

Browse Source 
【主	题】：代码格式化 较上版本 无任何变动
【描	述】：
	[原因]：
	[过程]：
	[影响]：
【结	束】

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
This commit is contained in:
tk 2024-07-02 18:24:51 +08:00
parent f3583e17e5
commit 811a89f8b2
9 changed files with 1453 additions and 1317 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
src/FoodDeliveryBase.cpp
View File

@ -3,7 +3,8 @@
* @description: 初始化 * @description: 初始化
*/ */
static const char *MOUDLENAME = "FC"; static const char *MOUDLENAME = "FC";
FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char* userUdpServerIP, uint32_t userUdpServerPort) { 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帐号 ssid = userSsid; // wifi帐号
password = userPassword; // wifi密码 password = userPassword; // wifi密码
@ -19,7 +20,8 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
_tm_mqttconnect = 0; _tm_mqttconnect = 0;
// 初始化飞控通讯串口 波特率 // 初始化飞控通讯串口 波特率
switch (mavlinkSerial) { //初始化指定 串口号 switch (mavlinkSerial)
{ // 初始化指定 串口号
case 0: case 0:
Serial.begin(57600); Serial.begin(57600);
break; break;
@ -31,7 +33,8 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
break; break;
} }
// 初始化声音模块串口 波特率 // 初始化声音模块串口 波特率
switch (voiceSerial) { //初始化指定 串口号 switch (voiceSerial)
{ // 初始化指定 串口号
case 0: case 0:
Serial.begin(115200); Serial.begin(115200);
break; break;
@ -50,47 +53,60 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
* @description: 日志打印 * @description: 日志打印
* @param {*} val 输出的信息 * @param {*} val 输出的信息
*/ */
void FoodCube::log(String val) { void FoodCube::log(String val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(char* val) { void FoodCube::log(char *val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(int val) { void FoodCube::log(int val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(IPAddress val) { void FoodCube::log(IPAddress val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(bool val) { void FoodCube::log(bool val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::logln(String val) { void FoodCube::logln(String val)
{
ESP_LOGD(MOUDLENAME, "%s", val); ESP_LOGD(MOUDLENAME, "%s", val);
} }
void FoodCube::logln(char* val) { void FoodCube::logln(char *val)
{
ESP_LOGD(MOUDLENAME, "%s", val); ESP_LOGD(MOUDLENAME, "%s", val);
} }
void FoodCube::logln(int val) { void FoodCube::logln(int val)
{
ESP_LOGD(MOUDLENAME, "%d", val); ESP_LOGD(MOUDLENAME, "%d", val);
} }
void FoodCube::logln(IPAddress val) { void FoodCube::logln(IPAddress val)
{
ESP_LOGD(MOUDLENAME, "%s", val.toString()); ESP_LOGD(MOUDLENAME, "%s", val.toString());
} }
void FoodCube::logln(bool val) { void FoodCube::logln(bool val)
{
Serial.print(val); Serial.print(val);
} }
/** /**
*@description: 取值 设置值 *@description: 取值 设置值
*/ */
bool FoodCube::getIsInit() { bool FoodCube::getIsInit()
{
return isInit; return isInit;
} }
void FoodCube::setIsInit(bool b) { void FoodCube::setIsInit(bool b)
{
isInit = b; isInit = b;
} }
String FoodCube::getMacAdd() { String FoodCube::getMacAdd()
{
return macAdd; return macAdd;
} }
@ -106,7 +122,8 @@ IPAddress subnet(255, 255, 255, 0);
IPAddress primaryDNS(8, 8, 8, 8); // optional IPAddress primaryDNS(8, 8, 8, 8); // optional
IPAddress secondaryDNS(8, 8, 4, 4); // optional IPAddress secondaryDNS(8, 8, 4, 4); // optional
void FoodCube::connectWifi() { void FoodCube::connectWifi()
{
// if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) { // if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) {
// ESP_LOGD(MOUDLENAME,"STA Failed to configure"); // ESP_LOGD(MOUDLENAME,"STA Failed to configure");
@ -164,13 +181,16 @@ bool FoodCube::checkWiFiStatus()
* @param {String[]} topicSub 主题数组 * @param {String[]} topicSub 主题数组
* @param {int} topicSubCount 数组总数 * @param {int} topicSubCount 数组总数
*/ */
void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { void FoodCube::connectMqtt(String topicSub[], int topicSubCount)
if (mqttClient->connected()) return; {
if (mqttClient->connected())
return;
/*尝试连接mqtt*/ /*尝试连接mqtt*/
if ((millis() - _tm_mqttconnect) > 3000) if ((millis() - _tm_mqttconnect) > 3000)
{ {
logln("connect_mqtt"); logln("connect_mqtt");
if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) { if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword))
{
logln("MQTT Server Connected."); logln("MQTT Server Connected.");
log("Server Address: "); log("Server Address: ");
logln(mqttServer); logln(mqttServer);
@ -181,7 +201,9 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
subscribeTopic("cmd", 1); subscribeTopic("cmd", 1);
delay(500); delay(500);
playText("服务器已连接"); playText("服务器已连接");
} else { }
else
{
// 失败返回状态码 // 失败返回状态码
log("MQTT Server Connect Failed. Client State:"); log("MQTT Server Connect Failed. Client State:");
logln(mqttClient->state()); logln(mqttClient->state());
@ -197,10 +219,14 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
* @param {String[]} topicSub 主题数组 * @param {String[]} topicSub 主题数组
* @param {int} topicSubCount 数组总数 * @param {int} topicSubCount 数组总数
*/ */
void FoodCube::mqttLoop(String topicSub[], int topicSubCount) { void FoodCube::mqttLoop(String topicSub[], int topicSubCount)
if (mqttClient->connected()) { //检测 如果开发板成功连接服务器 {
if (mqttClient->connected())
{ // 检测 如果开发板成功连接服务器
mqttClient->loop(); // 保持心跳 mqttClient->loop(); // 保持心跳
} else { // 如果开发板未能成功连接服务器 }
else
{ // 如果开发板未能成功连接服务器
connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器 connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器
} }
} }
@ -210,7 +236,8 @@ void FoodCube::mqttLoop(String topicSub[], int topicSubCount) {
* @param {String} topicString 主题名称 * @param {String} topicString 主题名称
* @param {int} Qos 1：重要 ps:响应时间快 * @param {int} Qos 1：重要 ps:响应时间快
*/ */
void FoodCube::subscribeTopic(String topicString, int Qos = 1) { void FoodCube::subscribeTopic(String topicString, int Qos = 1)
{
// 处理主题字符串 // 处理主题字符串
topicString = topicString + "/" + macAdd; topicString = topicString + "/" + macAdd;
char subTopic[topicString.length() + 1]; char subTopic[topicString.length() + 1];
@ -224,7 +251,8 @@ void FoodCube::subscribeTopic(String topicString, int Qos = 1) {
* @param {String} topicString 主题名称 * @param {String} topicString 主题名称
* @param {String} messageString 发布的内容 * @param {String} messageString 发布的内容
*/ */
void FoodCube::pubMQTTmsg(String topicString, String messageString) { void FoodCube::pubMQTTmsg(String topicString, String messageString)
{
// 处理主题字符串 // 处理主题字符串
topicString = topicString + "/" + macAdd; topicString = topicString + "/" + macAdd;
char publishTopic[topicString.length() + 1]; char publishTopic[topicString.length() + 1];
@ -242,9 +270,11 @@ void FoodCube::pubMQTTmsg(String topicString, String messageString) {
* @param {int} len 命令帧 的长度 * @param {int} len 命令帧 的长度
* @param {uint8_t} swSerial 串口选择 * @param {uint8_t} swSerial 串口选择
*/ */
void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) { void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial)
{
// 通过串口发送 // 通过串口发送
switch (swSerial) { //初始化指定 串口号 switch (swSerial)
{ // 初始化指定 串口号
case 0: case 0:
Serial.write(buf, len); Serial.write(buf, len);
break; break;
@ -267,7 +297,8 @@ void FoodCube::SetplayvolMax()
* @description: 声音播放 * @description: 声音播放
* @param {String} str 播放内容 * @param {String} str 播放内容
*/ */
void FoodCube::playText(String str,bool flying) { void FoodCube::playText(String str, bool flying)
{
String vstr; String vstr;
// 输出音量开到最大 // 输出音量开到最大
@ -300,7 +331,8 @@ void FoodCube::playText(String str,bool flying) {
command[index++] = 0x01; command[index++] = 0x01;
command[index++] = 0x04; command[index++] = 0x04;
/*帧命令 消息段*/ /*帧命令 消息段*/
for (int i = 0; i < vstr.length(); i++) { for (int i = 0; i < vstr.length(); i++)
{
command[index++] = (int)vstr[i]; command[index++] = (int)vstr[i];
} }
logln("playText"); logln("playText");
@ -320,28 +352,33 @@ void FoodCube::playText(String str,bool flying) {
* @description: 检查声音模块是否空闲 * @description: 检查声音模块是否空闲
* @return {uint8_t} 0x4E 78:系统忙 0x4F 79:系统空闲 * @return {uint8_t} 0x4E 78:系统忙 0x4F 79:系统空闲
*/ */
uint8_t FoodCube::chekVoiceMcu() { uint8_t FoodCube::chekVoiceMcu()
{
uint8_t serialData; uint8_t serialData;
uint8_t check[] = {0xFD, 0x00, 0x01, 0x21}; // 检查命令帧 uint8_t check[] = {0xFD, 0x00, 0x01, 0x21}; // 检查命令帧
SWrite(check, sizeof(check), voiceSerial); // 发送检查指令 SWrite(check, sizeof(check), voiceSerial); // 发送检查指令
switch (voiceSerial) { switch (voiceSerial)
{
case 0: case 0:
{ {
while (Serial.available()) { // 当串口接收到信息后 while (Serial.available())
{ // 当串口接收到信息后
serialData = Serial.read(); // 将接收到的信息使用read读取 serialData = Serial.read(); // 将接收到的信息使用read读取
} }
} }
break; break;
case 1: case 1:
{ {
while (Serial1.available()) { // 当串口接收到信息后 while (Serial1.available())
{ // 当串口接收到信息后
serialData = Serial1.read(); // 将接收到的信息使用read读取 serialData = Serial1.read(); // 将接收到的信息使用read读取
} }
} }
break; break;
case 2: case 2:
{ {
while (Serial2.available()) { // 当串口接收到信息后 while (Serial2.available())
{ // 当串口接收到信息后
serialData = Serial2.read(); // 将接收到的信息使用read读取 serialData = Serial2.read(); // 将接收到的信息使用read读取
} }
} }
@ -354,7 +391,8 @@ uint8_t FoodCube::chekVoiceMcu() {
/** /**
* @description: 声音模块停止合成 * @description: 声音模块停止合成
*/ */
void FoodCube::stopVoice() { void FoodCube::stopVoice()
{
uint8_t stop[] = {0xFD, 0x00, 0x01, 0x22}; // 停止合成命令帧 uint8_t stop[] = {0xFD, 0x00, 0x01, 0x22}; // 停止合成命令帧
SWrite(stop, sizeof(stop), voiceSerial); // 发送检查指令 SWrite(stop, sizeof(stop), voiceSerial); // 发送检查指令
} }
@ -364,11 +402,13 @@ void FoodCube::stopVoice() {
* @param {uint8_t[]} 命令帧 数组 * @param {uint8_t[]} 命令帧 数组
* @param {int} len 命令帧 的长度 * @param {int} len 命令帧 的长度
*/ */
uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) { uint16_t FoodCube::CRC16_cal(uint8_t *ptr, uint32_t len, uint16_t crc_init)
{
uint16_t crc, oldcrc16; uint16_t crc, oldcrc16;
uint8_t temp; uint8_t temp;
crc = crc_init; crc = crc_init;
while (len-- != 0) { while (len-- != 0)
{
temp = (crc >> 8) & 0xff; temp = (crc >> 8) & 0xff;
oldcrc16 = crc16_tab[*ptr ^ temp]; oldcrc16 = crc16_tab[*ptr ^ temp];
crc = (crc << 8) ^ oldcrc16; crc = (crc << 8) ^ oldcrc16;
@ -377,23 +417,25 @@ uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) {
return (crc); 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; uint16_t crc_result = 0;
crc_result = CRC16_cal(pbuf, len, 0); crc_result = CRC16_cal(pbuf, len, 0);
*p_result = crc_result; *p_result = crc_result;
} }
/** /**
* @description: 相机 向下 * @description: 相机 向下
*/ */
void FoodCube::Camera_action_down() { void FoodCube::Camera_action_down()
{
Camera_action_move(-100, 0); // 俯仰 向下 Camera_action_move(-100, 0); // 俯仰 向下
} }
/** /**
* @description: 相机 角度控制 * @description: 相机 角度控制
*/ */
void FoodCube::Camera_action_move(uint8_t vpitch,uint8_t vyaw ) { void FoodCube::Camera_action_move(uint8_t vpitch, uint8_t vyaw)
{
uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
command[8] = vyaw; // 旋转 command[8] = vyaw; // 旋转
command[9] = vpitch; // 俯仰 command[9] = vpitch; // 俯仰
@ -403,7 +445,8 @@ void FoodCube::Camera_action_move(uint8_t vpitch,uint8_t vyaw ) {
/** /**
* @description: 相机 角度控制 * @description: 相机 角度控制
*/ */
void FoodCube::Camera_action_zoom(uint8_t vzoom ) { void FoodCube::Camera_action_zoom(uint8_t vzoom)
{
uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00}; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
command[8] = vzoom; // 变焦 command[8] = vzoom; // 变焦
udpSendToCamera(command, sizeof(command)); udpSendToCamera(command, sizeof(command));
@ -412,7 +455,8 @@ void FoodCube::Camera_action_zoom(uint8_t vzoom ) {
/** /**
* @description: 相机 回中 * @description: 相机 回中
*/ */
void FoodCube::Camera_action_ret() { void FoodCube::Camera_action_ret()
{
uint8_t command1[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; uint8_t command1[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
udpSendToCamera(command1, sizeof(command1)); udpSendToCamera(command1, sizeof(command1));
uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
@ -424,14 +468,16 @@ void FoodCube::Camera_action_ret() {
* @param {uint8_t[]} 命令帧 数组 * @param {uint8_t[]} 命令帧 数组
* @param {int} len 命令帧 的长度 * @param {int} len 命令帧 的长度
*/ */
void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) { void FoodCube::udpSendToCamera(uint8_t *p_command, uint32_t len)
{
uint16_t result = 0; uint16_t result = 0;
uint16_t *p_result = &result; uint16_t *p_result = &result;
// 计算校检码 // 计算校检码
crc_check_16bites(p_command, len, p_result); crc_check_16bites(p_command, len, p_result);
// 加上校检码 // 加上校检码
uint8_t bytes[2 + len]; 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[i] = p_command[i];
} }
bytes[len] = static_cast<uint8_t>(result); // 低位 互换 bytes[len] = static_cast<uint8_t>(result); // 低位 互换
@ -455,24 +501,27 @@ void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) {
* @param {uint8_t} n 第几位 从0开始 * @param {uint8_t} n 第几位 从0开始
* @param {uint8_t} isOne 1:设置成1 0:设置成0 * @param {uint8_t} isOne 1:设置成1 0:设置成0
*/ */
String FoodCube::setNBit(String str, uint8_t n, uint8_t i) { String FoodCube::setNBit(String str, uint8_t n, uint8_t i)
{
char buf[10]; char buf[10];
uint16_t val = (uint8_t)str.toInt(); uint16_t val = (uint8_t)str.toInt();
if (i) { if (i)
{
val |= (1u << n); // 按位设置成1 val |= (1u << n); // 按位设置成1
} else { }
else
{
val &= ~(1u << n); // 按位设置成0 val &= ~(1u << n); // 按位设置成0
} }
sprintf(buf, "%d", val); sprintf(buf, "%d", val);
return buf; return buf;
} }
/** /**
* @description: 向飞控请求 指定数据 * @description: 向飞控请求 指定数据
*/ */
void FoodCube::mav_request_data() { void FoodCube::mav_request_data()
{
mavlink_message_t msg; mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// 以下注释 把数据流组合到一起 // 以下注释 把数据流组合到一起
@ -492,7 +541,8 @@ void FoodCube::mav_request_data() {
const int maxStreams = 1; // 遍历次数 （下面组合的长度） const int maxStreams = 1; // 遍历次数 （下面组合的长度）
const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_ALL}; // 请求的数据流组合 放到一个对象 后面进行遍历 const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_ALL}; // 请求的数据流组合 放到一个对象 后面进行遍历
const uint16_t MAVRates[maxStreams] = {0x01}; // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹 既每秒发送一次 const uint16_t MAVRates[maxStreams] = {0x01}; // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹 既每秒发送一次
for (int i = 0; i < maxStreams; i++) { for (int i = 0; i < maxStreams; i++)
{
// 向飞控发送请求 // 向飞控发送请求
mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1); mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
@ -505,24 +555,29 @@ void FoodCube::mav_request_data() {
* @description: 解析mavlink数据流 * @description: 解析mavlink数据流
* @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调 * @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
*/ */
void FoodCube::comm_receive(void (*pFun)( uint8_t)) { void FoodCube::comm_receive(void (*pFun)(uint8_t))
{
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t status; mavlink_status_t status;
switch (mavlinkSerial) { switch (mavlinkSerial)
{
case 0: case 0:
while (Serial.available() > 0) { // 判断串口缓存 是否有数据 while (Serial.available() > 0)
{ // 判断串口缓存 是否有数据
uint8_t c = Serial.read(); // 从缓存拿到数据 uint8_t c = Serial.read(); // 从缓存拿到数据
pFun(c); pFun(c);
} }
break; break;
case 1: case 1:
while (Serial1.available() > 0) { // 判断串口缓存 是否有数据 while (Serial1.available() > 0)
{ // 判断串口缓存 是否有数据
uint8_t c = Serial1.read(); // 从缓存拿到数据 uint8_t c = Serial1.read(); // 从缓存拿到数据
pFun(c); pFun(c);
} }
break; break;
case 2: case 2:
while (Serial2.available() > 0) { // 判断串口缓存 是否有数据 while (Serial2.available() > 0)
{ // 判断串口缓存 是否有数据
uint8_t c = Serial2.read(); // 从缓存拿到数据 uint8_t c = Serial2.read(); // 从缓存拿到数据
pFun(c); pFun(c);
} }
@ -534,7 +589,8 @@ void FoodCube::comm_receive(void (*pFun)( uint8_t)) {
* @description: 写入航点 第一步 “向飞控发送航点数量” * @description: 写入航点 第一步 “向飞控发送航点数量”
* @param {uint8_t} taskcount 航点数量 * @param {uint8_t} taskcount 航点数量
*/ */
void FoodCube::mav_mission_count(uint8_t taskcount) { void FoodCube::mav_mission_count(uint8_t taskcount)
{
mavlink_message_t msg; // mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包 // 设置飞行模式的数据包
@ -559,7 +615,8 @@ void FoodCube::mav_mission_count(uint8_t taskcount) {
* @param {double} y * @param {double} y
* @param {double} z * @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) { 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) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 写入航点数据包 // 写入航点数据包
@ -572,7 +629,8 @@ void FoodCube::mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint
/** /**
* @description: 向飞控发送 设置模式指令 * @description: 向飞控发送 设置模式指令
*/ */
void FoodCube::mav_set_mode(uint8_t SysState) { void FoodCube::mav_set_mode(uint8_t SysState)
{
mavlink_message_t msg; // mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包 // 设置飞行模式的数据包
@ -581,7 +639,8 @@ void FoodCube::mav_set_mode(uint8_t SysState) {
// 通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) { void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd)
{
mavlink_message_t msg; // mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd); mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd);
@ -589,7 +648,8 @@ void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) {
// 通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
void FoodCube::mav_send_text(uint8_t severity,const char *text) { void FoodCube::mav_send_text(uint8_t severity, const char *text)
{
return; return;
mavlink_message_t msg; // mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
@ -597,23 +657,23 @@ void FoodCube::mav_send_text(uint8_t severity,const char *text) {
mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, severity, text); mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, severity, text);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
printf("Send:"); printf("Send:");
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++)
printf("0x%02X ",buf[i]); ; {
printf("0x%02X ", buf[i]);
;
} }
printf(" \n"); printf(" \n");
// 通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
/** /**
* @description: 飞控 控制 * @description: 飞控 控制
* @param {uint8_t} controlType 3:加解锁模式控制 * @param {uint8_t} controlType 3:加解锁模式控制
* @param {uint16_t} param[] * @param {uint16_t} param[]
*/ */
void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) { void FoodCube::mav_command(uint8_t controlType, uint16_t param[])
{
mavlink_message_t msg; // mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包 // 设置飞行模式的数据包
@ -621,12 +681,14 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
cmd.target_system = 1; cmd.target_system = 1;
cmd.target_component = 1; cmd.target_component = 1;
cmd.confirmation = 0; cmd.confirmation = 0;
if (controlType == 3) { //飞机加解锁 if (controlType == 3)
{ // 飞机加解锁
cmd.command = MAV_CMD_COMPONENT_ARM_DISARM; cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
cmd.param1 = param[0]; // 0:加锁 1：解锁 cmd.param1 = param[0]; // 0:加锁 1：解锁
cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁 cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁
} }
if (controlType == 6) { //测试起飞 if (controlType == 6)
{ // 测试起飞
float p = (float)param[0]; float p = (float)param[0];
cmd.command = MAV_CMD_NAV_TAKEOFF; cmd.command = MAV_CMD_NAV_TAKEOFF;
cmd.param1 = 0; cmd.param1 = 0;
@ -637,10 +699,12 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
cmd.param6 = 0; cmd.param6 = 0;
cmd.param7 = p; // 起飞高度 cmd.param7 = p; // 起飞高度
} }
if (controlType == 8) { //降落 if (controlType == 8)
{ // 降落
cmd.command = MAV_CMD_NAV_LAND; cmd.command = MAV_CMD_NAV_LAND;
} }
if (controlType == 11) { //磁罗盘校准 if (controlType == 11)
{ // 磁罗盘校准
cmd.command = MAV_CMD_DO_START_MAG_CAL; cmd.command = MAV_CMD_DO_START_MAG_CAL;
cmd.param1 = 0; cmd.param1 = 0;
cmd.param2 = 1; cmd.param2 = 1;
@ -655,7 +719,8 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
* @description: 油门控制 * @description: 油门控制
* @param {uint16_t} chan[] 四个通道 * @param {uint16_t} chan[] 四个通道
*/ */
void FoodCube::mav_channels_override(uint16_t chan[]) { void FoodCube::mav_channels_override(uint16_t chan[])
{
mavlink_message_t msg; // mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 控制油门 // 控制油门

6
src/FoodDeliveryBase.h
View File

@ -16,7 +16,8 @@
#define MAVLINK_SYSTEM_ID 0xFF #define MAVLINK_SYSTEM_ID 0xFF
#define MAVLINK_COMPONENT_ID 0xBE #define MAVLINK_COMPONENT_ID 0xBE
class FoodCube { class FoodCube
{
public: public:
/*飞行航点任务相关属性*/ /*飞行航点任务相关属性*/
bool writeState = false; // 是否是写入状态 bool writeState = false; // 是否是写入状态
@ -99,8 +100,6 @@ private:
uint32_t udpServerPort; // 云台相机端口 uint32_t udpServerPort; // 云台相机端口
unsigned long _tm_mqttconnect; // mqtt上次连接时间 unsigned long _tm_mqttconnect; // mqtt上次连接时间
// 摄像头控制 校验代码 // 摄像头控制 校验代码
const uint16_t crc16_tab[256] = {0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, const uint16_t crc16_tab[256] = {0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
@ -138,5 +137,4 @@ 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); void crc_check_16bites(uint8_t *pbuf, uint32_t len, uint16_t *p_result);
}; };
#endif #endif

213
src/commser.cpp
View File

@ -47,17 +47,18 @@ Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题
// Ticker chanTicker; //定时向飞控 发送油门指定 // Ticker chanTicker; //定时向飞控 发送油门指定
/** /**
* @description: mqtt订阅主题 收到信息 的回调函数 * @description: mqtt订阅主题 收到信息 的回调函数
* @param {char*} topic 主题名称 msg/macadd * @param {char*} topic 主题名称 msg/macadd
* @param {byte*} topic 订阅获取的内容 * @param {byte*} topic 订阅获取的内容
* @param {unsigned int} length 内容的长度 * @param {unsigned int} length 内容的长度
*/ */
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
{
/*解构mqtt发过来的内容*/ /*解构mqtt发过来的内容*/
String jsonStr = ""; String jsonStr = "";
for (int i = 0; i < length; i++) { for (int i = 0; i < length; i++)
{
jsonStr += (char)payload[i]; jsonStr += (char)payload[i];
} }
/*解构内容*/ /*解构内容*/
@ -66,15 +67,19 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
// 遍历 JSON 对象 // 遍历 JSON 对象
String key; // 键 String key; // 键
JsonVariant value; // 值 JsonVariant value; // 值
for (JsonPair kv : doc.as<JsonObject>()) { for (JsonPair kv : doc.as<JsonObject>())
{
key = kv.key().c_str(); // 获取键 key = kv.key().c_str(); // 获取键
value = kv.value(); // 获取值 value = kv.value(); // 获取值
} }
/*根据订阅内容 键值 做具体操作*/ /*根据订阅内容 键值 做具体操作*/
if (key == "questAss") { //飞行航点任务 questAss if (key == "questAss")
{ // 飞行航点任务 questAss
String todo = value; // 转换值 String todo = value; // 转换值
writeRoute(todo); // 写入航点 writeRoute(todo); // 写入航点
} else if (key == "setPlaneState") { //设置飞机状态 }
else if (key == "setPlaneState")
{ // 设置飞机状态
/* /*
*其中topicPubMsg[10]既飞机状态的值 *其中topicPubMsg[10]既飞机状态的值
*二进制0000 0000 0000 0000 *二进制0000 0000 0000 0000
@ -101,59 +106,89 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
uint8_t count = obj["count"]; // 传过来 uint8_t count = obj["count"]; // 传过来
// 解构val数组参数 // 解构val数组参数
uint16_t param[count]; uint16_t param[count];
for (int i = 0; i < count; i++) { for (int i = 0; i < count; i++)
{
param[i] = obj["param"][i]; param[i] = obj["param"][i];
} }
// 标记飞机状态 // 标记飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state); topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
// 飞控执行 // 飞控执行
if (n == 3) { //3操作飞机加解锁 if (n == 3)
{ // 3操作飞机加解锁
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底 uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
fc.mav_channels_override(chan); // 控制油门 fc.mav_channels_override(chan); // 控制油门
fc.mav_set_mode(2); // 飞控设置成AltHold定高 fc.mav_set_mode(2); // 飞控设置成AltHold定高
fc.mav_command(n, param); fc.mav_command(n, param);
} else { }
else
{
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中 uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
fc.mav_channels_override(chan); // 控制油门 fc.mav_channels_override(chan); // 控制油门
} }
if (n == 6) { //6测试起飞 if (n == 6)
{ // 6测试起飞
fc.mav_set_mode(4); // 飞控设置成Guided引导模式 fc.mav_set_mode(4); // 飞控设置成Guided引导模式
fc.mav_command(n, param); // 起飞 fc.mav_command(n, param); // 起飞
} else if (n == 7) { //7 悬停 }
else if (n == 7)
{ // 7 悬停
fc.mav_set_mode(5); // 飞控设置成Loiter留待模式 fc.mav_set_mode(5); // 飞控设置成Loiter留待模式
} else if (n == 5) { //5 航点执行 }
else if (n == 5)
{ // 5 航点执行
fc.mav_set_mode(3); // 飞控设置成auto自动模式 fc.mav_set_mode(3); // 飞控设置成auto自动模式
} else if (n == 8) { //8降落* }
fc.mav_command(n, param); else if (n == 8)
} else if (n == 9) { //9返航 { // 8降落*
fc.mav_set_mode(6); //飞控设置成RTL返航
} else if (n == 11) { //11磁罗盘校准
fc.mav_command(n, param); fc.mav_command(n, param);
} }
} else if (key == "getPlaneState") { //获取飞机状态 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飞机状态 fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); // 终端主动get飞机状态
} else if (key == "resetState") { //恢复飞机为初始状态 }
else if (key == "resetState")
{ // 恢复飞机为初始状态
String todo = value; // 转换值 String todo = value; // 转换值
topicPubMsg[10] = todo; // 恢复初始状态 topicPubMsg[10] = todo; // 恢复初始状态
} else if (key == "chan1") { }
else if (key == "chan1")
{
uint16_t todo = value; // 转换值 uint16_t todo = value; // 转换值
fc.channels[0] = todo; // 恢复初始状态 fc.channels[0] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
} else if (key == "chan2") { }
else if (key == "chan2")
{
uint16_t todo = value; // 转换值 uint16_t todo = value; // 转换值
fc.channels[1] = todo; // 恢复初始状态 fc.channels[1] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
} else if (key == "chan3") { }
else if (key == "chan3")
{
uint16_t todo = value; // 转换值 uint16_t todo = value; // 转换值
fc.channels[2] = todo; // 恢复初始状态 fc.channels[2] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
} else if (key == "chan4") { }
else if (key == "chan4")
{
uint16_t todo = value; // 转换值 uint16_t todo = value; // 转换值
fc.channels[3] = todo; // 恢复初始状态 fc.channels[3] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
} else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置 }
else if (key == "hookConteroller")
{ // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
uint16_t todo = value; uint16_t todo = value;
switch(todo){ switch (todo)
{
case 0: case 0:
{ {
// 收钩 // 收钩
@ -180,7 +215,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
} }
break; break;
} }
} else if (key == "cameraController") { //云台相机控制 }
else if (key == "cameraController")
{ // 云台相机控制
String todoJson = value; // 转换值 String todoJson = value; // 转换值
/* json 反序列化 */ /* json 反序列化 */
DynamicJsonDocument doc(500); DynamicJsonDocument doc(500);
@ -191,19 +228,24 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
int8_t pitch = obj["pitch"]; int8_t pitch = obj["pitch"];
int8_t yaw = obj["yaw"]; int8_t yaw = obj["yaw"];
// 相机控制 // 相机控制
if (item == 0) { //0:一键回中 if (item == 0)
{ // 0:一键回中
uint8_t stopCommand[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; // 回中前 强制 俯仰旋转 停止 uint8_t stopCommand[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; // 回中前 强制 俯仰旋转 停止
size_t stopLen = sizeof(stopCommand); size_t stopLen = sizeof(stopCommand);
fc.udpSendToCamera(stopCommand, stopLen); fc.udpSendToCamera(stopCommand, stopLen);
uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
size_t len = sizeof(command); size_t len = sizeof(command);
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
} else if (item == 1) { //1:变焦 }
else if (item == 1)
{ // 1:变焦
uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00}; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
command[8] = val; command[8] = val;
size_t len = sizeof(command); size_t len = sizeof(command);
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
} else if (item == 2) { //2:俯仰 旋转 }
else if (item == 2)
{ // 2:俯仰 旋转
uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
command[8] = yaw; command[8] = yaw;
command[9] = pitch; command[9] = pitch;
@ -216,7 +258,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
* @description: 写入航点 * @description: 写入航点
* @param {String} todo mqtt订阅执行任务的Json字符串 * @param {String} todo mqtt订阅执行任务的Json字符串
*/ */
void writeRoute(String todoJson) { void writeRoute(String todoJson)
{
if (fc.writeState) // 如果正在写入状态 跳出 if (fc.writeState) // 如果正在写入状态 跳出
{ {
fc.logln("正在写航点"); // 提示正在写入中 fc.logln("正在写航点"); // 提示正在写入中
@ -235,10 +278,12 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
fc.mav_mission_count(taskcount); // 向飞控请求写入航点的数量 fc.mav_mission_count(taskcount); // 向飞控请求写入航点的数量
fc.writeState = true; // 锁定写入状态 fc.writeState = true; // 锁定写入状态
// 监听飞控航点写入情况 // 监听飞控航点写入情况
while (true) { while (true)
{
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
if (fc.missionArkType == 0) { //写入成功 if (fc.missionArkType == 0)
{ // 写入成功
fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态
fc.logln("misson_bingo..."); fc.logln("misson_bingo...");
// 改变飞机状态 // 改变飞机状态
@ -246,7 +291,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
break; break;
} else if (fc.missionArkType > 0) { //写入失败 }
else if (fc.missionArkType > 0)
{ // 写入失败
fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态
fc.logln("misson_error..."); fc.logln("misson_error...");
// 改变飞机状态 // 改变飞机状态
@ -256,7 +303,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
return; // 写入失败 中断函数 return; // 写入失败 中断函数
} }
// 飞控返回 新的写入航点序号 // 飞控返回 新的写入航点序号
if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) { if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1)
{
// 从订阅信息里拿航点参数 // 从订阅信息里拿航点参数
uint8_t frame = obj["tasks"][fc.writeSeq]["frame"]; uint8_t frame = obj["tasks"][fc.writeSeq]["frame"];
uint8_t command = obj["tasks"][fc.writeSeq]["command"]; uint8_t command = obj["tasks"][fc.writeSeq]["command"];
@ -270,7 +318,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
double y = obj["tasks"][fc.writeSeq]["y"]; double y = obj["tasks"][fc.writeSeq]["y"];
double z = obj["tasks"][fc.writeSeq]["z"]; double z = obj["tasks"][fc.writeSeq]["z"];
String str = obj["tasks"][fc.writeSeq]["sound"]; String str = obj["tasks"][fc.writeSeq]["sound"];
if (str != "") { if (str != "")
{
fc.questVoiceStr = str; fc.questVoiceStr = str;
} }
fc.logln("frame--"); fc.logln("frame--");
@ -289,14 +338,17 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
* @param {mavlink_status_t*} pStatus * @param {mavlink_status_t*} pStatus
* @param {uint8_t} c 串口读取的缓存 * @param {uint8_t} c 串口读取的缓存
*/ */
void mavlink_receiveCallback(uint8_t c) { void mavlink_receiveCallback(uint8_t c)
{
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t status; mavlink_status_t status;
// 尝试从数据流里 解析数据 // 尝试从数据流里 解析数据
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{ // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
// 通过msgid来判断 数据流的类别 // 通过msgid来判断 数据流的类别
char buf[10]; char buf[10];
switch (msg.msgid) { switch (msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
{ {
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
@ -304,18 +356,25 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
sprintf(buf, "%d", heartbeat.base_mode); // 飞控心跳状态 sprintf(buf, "%d", heartbeat.base_mode); // 飞控心跳状态
topicPubMsg[0] = buf; // 心跳主题 topicPubMsg[0] = buf; // 心跳主题
// 从心跳里判断 飞机是否解锁 解锁改变飞机状态 // 从心跳里判断 飞机是否解锁 解锁改变飞机状态
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (heartbeat.base_mode & 128)
{ // 从心跳里面 判断已经解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
} else { //心跳里面 判断没有解锁 }
else
{ // 心跳里面 判断没有解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点 if ((uint8_t)topicPubMsg[10].toInt() & 4)
{ // 如果已经写入了航点
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态
} else { }
else
{
topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态 topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态
} }
} }
// 从心跳里面 判断飞机当前的模式 // 从心跳里面 判断飞机当前的模式
switch (heartbeat.custom_mode) { switch (heartbeat.custom_mode)
{
case 0: case 0:
{ {
topicPubMsg[12] = "Stabilize自稳"; topicPubMsg[12] = "Stabilize自稳";
@ -371,17 +430,20 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据 mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
// 电压 // 电压
sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000); sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
if (topicPubMsg[1] != buf) { // 有更新 则更新数据 if (topicPubMsg[1] != buf)
{ // 有更新 则更新数据
topicPubMsg[1] = buf; topicPubMsg[1] = buf;
} }
// 电流 // 电流
sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); sprintf(buf, "%.2f", (double)sys_status.current_battery / 100);
if (topicPubMsg[2] != buf) { if (topicPubMsg[2] != buf)
{
topicPubMsg[2] = buf; topicPubMsg[2] = buf;
} }
// 电池电量 // 电池电量
sprintf(buf, "%d", sys_status.battery_remaining); sprintf(buf, "%d", sys_status.battery_remaining);
if (topicPubMsg[3] != buf) { if (topicPubMsg[3] != buf)
{
topicPubMsg[3] = buf; topicPubMsg[3] = buf;
} }
} }
@ -407,7 +469,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
mavlink_msg_global_position_int_decode(&msg, &global_position_int); mavlink_msg_global_position_int_decode(&msg, &global_position_int);
// 高度 // 高度
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000); sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
if (topicPubMsg[4] != buf) { if (topicPubMsg[4] != buf)
{
topicPubMsg[4] = buf; topicPubMsg[4] = buf;
} }
//{经度，维度，海拔高度} //{经度，维度，海拔高度}
@ -415,7 +478,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
global_position_int.lon, global_position_int.lon,
global_position_int.lat, global_position_int.lat,
(double)global_position_int.alt / 1000); (double)global_position_int.alt / 1000);
if (topicPubMsg[15] != buf) { if (topicPubMsg[15] != buf)
{
topicPubMsg[15] = buf; topicPubMsg[15] = buf;
} }
} }
@ -427,7 +491,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
mavlink_msg_vfr_hud_decode(&msg, &vfr_hud); mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
// 对地速度 ps:飞行速度 // 对地速度 ps:飞行速度
sprintf(buf, "%.2f", vfr_hud.groundspeed); sprintf(buf, "%.2f", vfr_hud.groundspeed);
if (topicPubMsg[5] != buf) { if (topicPubMsg[5] != buf)
{
topicPubMsg[5] = buf; topicPubMsg[5] = buf;
} }
} }
@ -439,21 +504,25 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int); mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
// 卫星数 // 卫星数
sprintf(buf, "%d", gps_raw_int.satellites_visible); sprintf(buf, "%d", gps_raw_int.satellites_visible);
if (topicPubMsg[6] != buf) { if (topicPubMsg[6] != buf)
{
topicPubMsg[6] = buf; topicPubMsg[6] = buf;
} }
// 纬度 // 纬度
sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000); sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
if (topicPubMsg[7] != buf) { if (topicPubMsg[7] != buf)
{
topicPubMsg[7] = buf; topicPubMsg[7] = buf;
} }
// 经度 // 经度
sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000); sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
if (topicPubMsg[8] != buf) { if (topicPubMsg[8] != buf)
{
topicPubMsg[8] = buf; topicPubMsg[8] = buf;
} }
// 卫星状态 // 卫星状态
switch (gps_raw_int.fix_type) { switch (gps_raw_int.fix_type)
{
case 0: case 0:
{ {
topicPubMsg[9] = "No Fix"; topicPubMsg[9] = "No Fix";
@ -534,22 +603,29 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
/** /**
* @description: 发送主题线程 * @description: 发送主题线程
*/ */
void pubThread() { void pubThread()
{
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
// 创建一个JSON对象 // 创建一个JSON对象
DynamicJsonDocument doc(2000); // 缓冲区 DynamicJsonDocument doc(2000); // 缓冲区
// 遍历 有更新的数据 组成一个json对象 // 遍历 有更新的数据 组成一个json对象
for (int i = 0; i < topicPubCount; i++) { for (int i = 0; i < topicPubCount; i++)
if (topicPubMsg[i] != oldMsg[i]) { {
if (i == 0) { //心跳包 每每向心跳主题发布信息 if (topicPubMsg[i] != oldMsg[i])
{
if (i == 0)
{ // 心跳包 每每向心跳主题发布信息
// 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 // 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
if (fc.getIsInit()) { if (fc.getIsInit())
{
fc.setIsInit(false); fc.setIsInit(false);
fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容 fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容
} }
// 设置对象成员 ps:心跳 // 设置对象成员 ps:心跳
doc[topicPub[0]] = topicPubMsg[0]; doc[topicPub[0]] = topicPubMsg[0];
} else { //非心跳 有更新 录入成员 }
else
{ // 非心跳 有更新 录入成员
doc[topicPub[i]] = topicPubMsg[i]; doc[topicPub[i]] = topicPubMsg[i];
oldMsg[i] = topicPubMsg[i]; oldMsg[i] = topicPubMsg[i];
} }
@ -570,14 +646,19 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
/** /**
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/ */
void flashThread() { void flashThread()
if (digitalRead(23) == LOW) { {
if (isPush) { //点击之后 if (digitalRead(23) == LOW)
{
if (isPush)
{ // 点击之后
// 请求注册 ps:发送esp8266的物理地址 到对频主题 // 请求注册 ps:发送esp8266的物理地址 到对频主题
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
} }
isPush = false; // 复位按钮 isPush = false; // 复位按钮
} else { }
else
{
// FLASH按下状态 // FLASH按下状态
isPush = true; isPush = true;
} }
@ -586,13 +667,15 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
/** /**
* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
*/ */
void mavThread() { void mavThread()
{
fc.mav_request_data(); fc.mav_request_data();
} }
/** /**
* @description: 向飞控 发送油门指令 * @description: 向飞控 发送油门指令
*/ */
void chanThread() { void chanThread()
{
// mav_channels_override(channels); // mav_channels_override(channels);
} }

1
src/config.h
View File

@ -42,7 +42,6 @@
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms #define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms #define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
enum HookStatus enum HookStatus
{ {
HS_UnInit, // 还未初始化 HS_UnInit, // 还未初始化

62
src/main.cpp
View File

@ -88,12 +88,6 @@ unsigned long _weightalign_begintm; //校准开始时间
Initstatus initstatus; Initstatus initstatus;
unsigned long _tm_waitinit; unsigned long _tm_waitinit;
void setup() void setup()
{ {
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启 WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
@ -107,9 +101,6 @@ void setup()
wei_offset = preferences.getLong("wei_offset", 0); wei_offset = preferences.getLong("wei_offset", 0);
ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset); ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset);
// 初始化按钮 // 初始化按钮
button_up.attachClick(upbtn_click); button_up.attachClick(upbtn_click);
button_up.attachDoubleClick(upbtn_dbclick); button_up.attachDoubleClick(upbtn_dbclick);
@ -162,9 +153,6 @@ void setup()
flashTicker.start(); // 监听 按flash键时 主动发布对频主题 flashTicker.start(); // 监听 按flash键时 主动发布对频主题
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
// if (motocontrol.getstatus()==MS_Stop) // if (motocontrol.getstatus()==MS_Stop)
// { // {
// //slowup(); // //slowup();
@ -196,7 +184,8 @@ void checkstatus()
{ {
if (abs(pullweight) > 100) if (abs(pullweight) > 100)
_checkweighttimes++; _checkweighttimes++;
else _checkweighttimes=0; else
_checkweighttimes = 0;
// ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight); // ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
if (_checkweighttimes > 10) if (_checkweighttimes > 10)
{ {
@ -204,13 +193,12 @@ void checkstatus()
// ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight); // ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量"); fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量");
} }
}else }
else
{ {
_checkweightcal = false; _checkweightcal = false;
} }
} }
} }
_lasthooktatus = vhooktatus; _lasthooktatus = vhooktatus;
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2); // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
@ -272,7 +260,9 @@ bool check_tare()
motocontrol.weightalign(true); motocontrol.weightalign(true);
ESP_LOGD(MOUDLENAME, "check_tare ok: %d,offset:%d", pullweight, wei_offset); ESP_LOGD(MOUDLENAME, "check_tare ok: %d,offset:%d", pullweight, wei_offset);
return true; return true;
}else _needweightalign = true; }
else
_needweightalign = true;
} }
else else
{ {
@ -335,7 +325,8 @@ void checkinited()
_needweightalign = false; _needweightalign = false;
// fc.playText("挂钩已锁定"); // fc.playText("挂钩已锁定");
initstatus = IS_OK; initstatus = IS_OK;
}else }
else
{ {
ESP_LOGD(MOUDLENAME, "begin_tare"); ESP_LOGD(MOUDLENAME, "begin_tare");
begin_tare(); begin_tare();
@ -349,8 +340,8 @@ void checkinited()
{ {
// fc.playText("挂钩已锁定"); // fc.playText("挂钩已锁定");
initstatus = IS_OK; initstatus = IS_OK;
}else }
if (_weightalign_status==WAS_Alignfault) else if (_weightalign_status == WAS_Alignfault)
{ {
ESP_LOGD(MOUDLENAME, "weightalign fault! again"); ESP_LOGD(MOUDLENAME, "weightalign fault! again");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
@ -358,7 +349,6 @@ void checkinited()
break; break;
} }
/* /*
case Initstatus::IS_LengthZero: case Initstatus::IS_LengthZero:
{ {
@ -414,7 +404,6 @@ void loop()
// pubTicker.update(); //定时 发布主题 // pubTicker.update(); //定时 发布主题
// mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 // mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
// sercomm.getcommand(); // 得到控制命令 // sercomm.getcommand(); // 得到控制命令
button_checktop.tick(); // 按钮 button_checktop.tick(); // 按钮
button_down.tick(); // 按钮 button_down.tick(); // 按钮
@ -456,7 +445,6 @@ void loop()
fc.mqttLoop(topicPub, topicPubCount); fc.mqttLoop(topicPub, topicPubCount);
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
delay(1); delay(1);
} }
// 在核心0上执行耗时长的低优先级的 // 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters) void Task1(void *pvParameters)
@ -558,8 +546,6 @@ void showledidel()
led_show(255, 255, 255); // 白色 led_show(255, 255, 255); // 白色
break; break;
} }
} }
/* /*
@ -642,7 +628,8 @@ void down_action(float motospeed,float length = 0.0f)
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
{ {
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
}else }
else
{ {
motocontrol.setspeed(motospeed); motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Down, length); motocontrol.moto_run(MotoStatus::MS_Down, length);
@ -661,7 +648,8 @@ void up_action(float motospeed)
motocontrol.setspeed(motospeed); motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Up); motocontrol.moto_run(MotoStatus::MS_Up);
fc.playText("收线"); fc.playText("收线");
}else }
else
motocontrol.stoprun(); motocontrol.stoprun();
} }
// 放线按钮--单击 // 放线按钮--单击
@ -684,7 +672,8 @@ void downbtn_pressstart()
ESP_LOGD(MOUDLENAME, "Down_pressstart"); ESP_LOGD(MOUDLENAME, "Down_pressstart");
// 两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce > 2)
btn_pressatonce = 2;
if (btn_pressatonce == 2) if (btn_pressatonce == 2)
{ {
btn_presssatonce(); btn_presssatonce();
@ -697,7 +686,8 @@ void downbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME, "Down_pressend"); ESP_LOGD(MOUDLENAME, "Down_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce < 0)
btn_pressatonce = 0;
// 不是恢复自动放线抬起按键就停止 // 不是恢复自动放线抬起按键就停止
if (!motocontrol.getcontrolstatus().is_autogoodsdown) if (!motocontrol.getcontrolstatus().is_autogoodsdown)
motocontrol.stoprun(); motocontrol.stoprun();
@ -731,7 +721,8 @@ void upbtn_pressstart()
ESP_LOGD(MOUDLENAME, "UP_pressstart"); ESP_LOGD(MOUDLENAME, "UP_pressstart");
// 两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce > 2)
btn_pressatonce = 2;
if (btn_pressatonce == 2) if (btn_pressatonce == 2)
{ {
btn_presssatonce(); btn_presssatonce();
@ -744,7 +735,8 @@ void upbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME, "UP_pressend"); ESP_LOGD(MOUDLENAME, "UP_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce < 0)
btn_pressatonce = 0;
motocontrol.stoprun(); motocontrol.stoprun();
} }
@ -757,7 +749,8 @@ void Hook_autodown(float length_cm)
ESP_LOGD(MOUDLENAME, "Hook_autodown %.2f cm", length_cm); ESP_LOGD(MOUDLENAME, "Hook_autodown %.2f cm", length_cm);
fc.Camera_action_down(); fc.Camera_action_down();
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
}else }
else
ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked "); ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked ");
} }
void Hook_stop() void Hook_stop()
@ -771,7 +764,8 @@ void Hook_resume()
{ {
ESP_LOGD(MOUDLENAME, "Hook_resume"); ESP_LOGD(MOUDLENAME, "Hook_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
}else }
else
ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop "); ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop ");
} }
void Hook_recovery() void Hook_recovery()
@ -812,5 +806,3 @@ void testbtn_click()
} }
//////////////////////////////MQTT_语音_MAVLINK 部分 //////////////////////////////MQTT_语音_MAVLINK 部分

6
src/motocontrol.cpp
View File

@ -98,8 +98,6 @@ String Motocontrol::gethooktatus_str(bool chstr)
return hookstatusstr; return hookstatusstr;
else else
return hookstatusstr_en; return hookstatusstr_en;
} }
void Motocontrol::checkgoods() // 检测是否超重 void Motocontrol::checkgoods() // 检测是否超重
@ -199,7 +197,6 @@ int16_t Motocontrol::getlength() // 得到长度
return 0; return 0;
} }
// 重量传感器已经校准 // 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign) void Motocontrol::weightalign(bool weightalign)
{ {
@ -432,7 +429,8 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdownresume() void Motocontrol::moto_goodsdownresume()
{ {
if (!_controlstatus.is_autogoodsdown) return; if (!_controlstatus.is_autogoodsdown)
return;
if (_hooksstatus == HS_Stop) if (_hooksstatus == HS_Stop)
{ {
_runspeed = _goods_speed; _runspeed = _goods_speed;

1
src/motocontrol.h
View File

@ -111,6 +111,7 @@ private:
void unlockservo(); void unlockservo();
void set_hook_speed(float hooksspeed); void set_hook_speed(float hooksspeed);
void sethooksstatus(HookStatus hookstatus); void sethooksstatus(HookStatus hookstatus);
public: public:
Motocontrol(); // 构造函数 Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数 ~Motocontrol(); // 析构函数