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

1.合并小田新的文件,加入相机云台控制

Browse Source 
2.修改钩子控制部分
3.反馈给服务器钩子文字状态
This commit is contained in:
pxzleo 2023-06-19 15:26:16 +08:00
parent 2ffb029774
commit 96df77d785
5 changed files with 236 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
src/FoodDeliveryBase.cpp
View File

@ -3,7 +3,7 @@
/** /**
* @description: 初始化 * @description: 初始化
*/ */
FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial) { 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密码
@ -13,6 +13,8 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
mqttPassword = userMqttPassword; //mqtt密码 mqttPassword = userMqttPassword; //mqtt密码
mavlinkSerial = userMavlinkSerial; //mavlink用的串口 mavlinkSerial = userMavlinkSerial; //mavlink用的串口
voiceSerial = userVoiceSerial; //声音模块用的串口 voiceSerial = userVoiceSerial; //声音模块用的串口
udpServerIP = userUdpServerIP; //云台相机ip
udpServerPort = userUdpServerPort; //云台相机端口
//初始化飞控通讯串口 波特率 //初始化飞控通讯串口 波特率
switch (mavlinkSerial) { //初始化指定 串口号 switch (mavlinkSerial) { //初始化指定 串口号
@ -113,6 +115,7 @@ void FoodCube::connectWifi() {
macAdd.replace(":", ""); //板子的物理地址 并且去除冒号 macAdd.replace(":", ""); //板子的物理地址 并且去除冒号
log("macAdd: "); log("macAdd: ");
logln(macAdd); logln(macAdd);
playText("歪佛哎,已连接");
delay(500); delay(500);
} }
@ -134,6 +137,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题 for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题
subscribeTopic(topicSub[i], 1); subscribeTopic(topicSub[i], 1);
} }
playText("哎木可优踢踢,已连接");
} else { } else {
//失败返回状态码 //失败返回状态码
log("MQTT Server Connect Failed. Client State:"); log("MQTT Server Connect Failed. Client State:");
@ -207,6 +211,7 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
} }
} }
/** /**
* @description: 声音播放 * @description: 声音播放
* @param {String} str 播放内容 * @param {String} str 播放内容
@ -286,6 +291,52 @@ void FoodCube::stopVoice() {
SWrite(stop, sizeof(stop), voiceSerial); //发送检查指令 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 * @description: 按位设置 1 or 0
* @param {String} str 要设置的值 * @param {String} str 要设置的值

50
src/FoodDeliveryBase.h
View File

@ -11,6 +11,8 @@
#include "ArduinoJson.h" #include "ArduinoJson.h"
/*异步库*/ /*异步库*/
#include "Ticker.h" #include "Ticker.h"
/*udp发送*/
#include "WiFiUdp.h"
class FoodCube { class FoodCube {
public: public:
@ -22,7 +24,7 @@ public:
/*前端模拟遥控的油门通道*/ /*前端模拟遥控的油门通道*/
uint16_t channels[4] = { 1500, 1500, 1500, 1500 }; 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); 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(String val);
void log(char* val); void log(char* val);
@ -47,7 +49,7 @@ public:
void subscribeTopic(String topicString, int Qos); void subscribeTopic(String topicString, int Qos);
void pubMQTTmsg(String topicString, String messageString); void pubMQTTmsg(String topicString, String messageString);
/*串口输出*/ /*串口输出*/
void SWrite(uint8_t buf[], int len,uint8_t swSerial); void SWrite(uint8_t buf[], int len, uint8_t swSerial);
/*声音模块控制*/ /*声音模块控制*/
void playText(String str); void playText(String str);
uint8_t chekVoiceMcu(); uint8_t chekVoiceMcu();
@ -61,6 +63,8 @@ public:
void mav_set_mode(uint8_t SysState); void mav_set_mode(uint8_t SysState);
void mav_command(uint8_t controlType, uint16_t param[]); void mav_command(uint8_t controlType, uint16_t param[]);
void mav_channels_override(uint16_t chan[]); void mav_channels_override(uint16_t chan[]);
/*云台相机控制*/
void udpSendToCamera(uint8_t* p_command, uint32_t len);
private: private:
char* ssid; //wifi帐号 char* ssid; //wifi帐号
@ -76,5 +80,47 @@ private:
WiFiClient wifiClient; //网络客户端 WiFiClient wifiClient; //网络客户端
IPAddress localIp; //板子的IP地址 IPAddress localIp; //板子的IP地址
String macAdd; //板子的物理地址(已去掉冒号分隔符) 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 #endif

131
src/main.cpp
View File

@ -56,7 +56,7 @@ void Task1(void *pvParameters);
void led_show(CRGB ledcolor); void led_show(CRGB ledcolor);
// void Task1code( void *pvParameters ); // void Task1code( void *pvParameters );
void showledidel(); void showledidel();
int pullweight = 0; int pullweight = 0; //检测重量-克
int8_t btn_pressatonce=0; //是否同时按下 int8_t btn_pressatonce=0; //是否同时按下
unsigned long _tm_bengstop; unsigned long _tm_bengstop;
bool _bengstop = false; bool _bengstop = false;
@ -88,32 +88,35 @@ unsigned long _tm_waitinit;
/*项目对象*/ /*项目对象*/
//char* ssid = "szdot"; //wifi帐号 //char* ssid = "szdot"; //wifi帐号
//char* password = "Ttaj@#*.com"; //wifi密码 //char* password = "Ttaj@#*.com"; //wifi密码
char* ssid = "flicube"; //wifi帐号 char* ssid = (char*)"flicube"; //wifi帐号
char* password = "fxmf0622"; //wifi密码 char* password = (char*)"fxmf0622"; //wifi密码
char* mqttServer = "szdot.top"; //mqtt地址 char* mqttServer = (char*)"szdot.top"; //mqtt地址
int mqttPort = 1883; //mqtt端口 int mqttPort = 1883; //mqtt端口
char* mqttName = "admin"; //mqtt帐号 char* mqttName = (char*)"admin"; //mqtt帐号
char* mqttPassword = "123456"; //mqtt密码 char* mqttPassword = (char*)"123456"; //mqtt密码
uint8_t mavlinkSerial = 2; //飞控占用的串口号 uint8_t mavlinkSerial = 2; //飞控占用的串口号
uint8_t voiceSerial = 1; //声音模块占用串口 uint8_t voiceSerial = 1; //声音模块占用串口
char* udpServerIP = (char*) "192.168.8.100"; //云台相机ip
uint32_t udpServerPort = 37260; //云台相机端口
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length); void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
void mavThread(); void mavThread();
void pubThread(); void pubThread();
void flashThread() ; void flashThread() ;
void writeRoute(String topicStr); void writeRoute(String topicStr);
void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) ; void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) ;
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial); //创建项目对象 FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort); //创建项目对象
/*订阅 主题*/ /*订阅 主题*/
//0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 //0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 8:钩子控制
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4" }; //订阅主题 String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" }; //订阅主题
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数
/*有更新主动发送 主题*/ /*有更新主动发送 主题*/
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 //0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode" }; String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus" };
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数
String topicPubMsg[13]; //发送数据存放 对应topicPub String topicPubMsg[15]; //发送数据存放 对应topicPub
String oldMsg[13]; //记录旧的数据 用来对比有没有更新 String oldMsg[15]; //记录旧的数据 用来对比有没有更新
/*触发发送 主题*/ /*触发发送 主题*/
//0:对频信息 //0:对频信息
String topicHandle[] = { "crosFrequency" }; String topicHandle[] = { "crosFrequency" };
@ -215,7 +218,9 @@ void showinfo()
// if (pullweight > 10) // if (pullweight > 10)
// printf("PullWeight:%d\n", pullweight); // printf("PullWeight:%d\n", pullweight);
// HookStatus hs=motocontrol.gethooktatus() ; topicPubMsg[14]=motocontrol.gethooktatus_str() ;
topicPubMsg[13]=pullweight;
// control_status_t cs=motocontrol.getcontrolstatus() ; // control_status_t cs=motocontrol.getcontrolstatus() ;
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus); // printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
@ -351,7 +356,6 @@ void loop()
} }
// 检测执行初始化工作 // 检测执行初始化工作
checkinited(); checkinited();
/////////////////////////////////MQTT_语音_MAVLINK 部分 /////////////////////////////////MQTT_语音_MAVLINK 部分
/*从飞控拿数据*/ /*从飞控拿数据*/
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
@ -377,6 +381,8 @@ void Task1(void *pvParameters)
scale.tare(); scale.tare();
} }
pullweight = get_pullweight(); pullweight = get_pullweight();
// printf("pullweight: %d \n", pullweight);
vTaskDelay(10); vTaskDelay(10);
} }
} }
@ -657,7 +663,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
/*订阅回调主体*/ /*订阅回调主体*/
if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss
writeRoute(topicStr); //写入航点 writeRoute(topicStr); //写入航点
} } else
if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState
/* /*
*其中topicPubMsg[10]既飞机状态的值 *其中topicPubMsg[10]既飞机状态的值
@ -721,29 +727,74 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
} }
if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState
fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态 fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态
} }else
if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState
topicPubMsg[10] = "1"; //恢复初始状态 topicPubMsg[10] = "1"; //恢复初始状态
} }else
if (cutTopic == topicSub[4]) { //4:油门通道1 if (cutTopic == topicSub[4]) { //4:油门通道1
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
fc.channels[0] = strInt; //恢复初始状态 fc.channels[0] = strInt; //恢复初始状态
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_channels_override(fc.channels); //油门控制
} }else
if (cutTopic == topicSub[5]) { //5:油门通道2 if (cutTopic == topicSub[5]) { //5:油门通道2
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
fc.channels[1] = strInt; //恢复初始状态 fc.channels[1] = strInt; //恢复初始状态
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_channels_override(fc.channels); //油门控制
} }else
if (cutTopic == topicSub[6]) { //6:油门通道3 if (cutTopic == topicSub[6]) { //6:油门通道3
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
fc.channels[2] = strInt; //恢复初始状态 fc.channels[2] = strInt; //恢复初始状态
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_channels_override(fc.channels); //油门控制
} }else
if (cutTopic == topicSub[7]) { //7:油门通道4 if (cutTopic == topicSub[7]) { //7:油门通道4
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
fc.channels[3] = strInt; //恢复初始状态 fc.channels[3] = strInt; //恢复初始状态
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_channels_override(fc.channels); //油门控制
}else
if (cutTopic == topicSub[8]) { //8:钩子控制
uint16_t strInt = (uint16_t)topicStr.toInt(); //
printf("hookcontrol command: %d \n", strInt);
switch (strInt)
{
case 0: //收
/* code */
break;
case 1: //放
/* code */
break;
case 2: //暂停
/* code */
break;
case 3: //继续
/* code */
break;
}
}else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController
// json 反序列化
DynamicJsonDocument doc(0x2FFF);
deserializeJson(doc, topicStr);
JsonObject obj = doc.as<JsonObject>();
//相机控制
size_t len;
if (obj["item"] == "0") { //0:一键回中
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
len = sizeof(command);
fc.udpSendToCamera(command, len);
} else if (obj["item"] == "1") { //1:变焦
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x0 };
// strval=(obj["val"] as String);
command[8] = (uint8_t)obj["val"];
len = sizeof(command);
fc.udpSendToCamera(command, len);
} else if (obj["item"] == "2") { //2:俯仰,旋转
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
command[9] = (uint8_t)obj["pitch"]; //俯仰
command[8] = (uint8_t)obj["yaw"]; //旋转
len = sizeof(command);
fc.udpSendToCamera(command, len);
}
} }
} }
@ -754,7 +805,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
void writeRoute(String topicStr) { void writeRoute(String topicStr) {
if (fc.writeState) // 如果正在写入状态 跳出 if (fc.writeState) // 如果正在写入状态 跳出
{ {
fc.logln("正在写航点"); // 提示正在写入中 fc.logln((char*)"正在写航点"); // 提示正在写入中
return; return;
} }
//改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
@ -775,7 +826,7 @@ void writeRoute(String topicStr) {
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((char*)"misson_bingo...");
//改变飞机状态 //改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
@ -783,7 +834,7 @@ void writeRoute(String topicStr) {
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((char*)"misson_error...");
//改变飞机状态 //改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
@ -804,7 +855,7 @@ void writeRoute(String topicStr) {
double x = obj["tasks"][fc.writeSeq]["x"]; double x = obj["tasks"][fc.writeSeq]["x"];
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"];
fc.logln("frame--"); fc.logln((char*)"frame--");
fc.logln(frame); fc.logln(frame);
//写入航点 //写入航点
fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
@ -835,12 +886,18 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
//从心跳里判断 飞机是否解锁 解锁改变飞机状态 //从心跳里判断 飞机是否解锁 解锁改变飞机状态
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁
if (!curr_armed) if (!curr_armed)
{
printf("armed\n");
fc.playText("已解锁"); fc.playText("已解锁");
}
curr_armed=true; curr_armed=true;
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
} else { } else {
if (curr_armed) if (curr_armed)
{
printf("disarm\n");
fc.playText("已加锁"); fc.playText("已加锁");
}
curr_armed=false; //心跳里面 判断没有解锁 curr_armed=false; //心跳里面 判断没有解锁
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) { //如果已经写入了航点
@ -857,42 +914,42 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
switch (heartbeat.custom_mode) { switch (heartbeat.custom_mode) {
case 0: case 0:
{ {
topicPubMsg[12] = "姿态模式"; topicPubMsg[12] = "姿态";
} }
break; break;
case 2: case 2:
{ {
topicPubMsg[12] = "定高模式"; topicPubMsg[12] = "定高";
} }
break; break;
case 3: case 3:
{ {
topicPubMsg[12] = "自动模式"; topicPubMsg[12] = "自动";
} }
break; break;
case 4: case 4:
{ {
topicPubMsg[12] = "引导模式"; topicPubMsg[12] = "指点";
} }
break; break;
case 5: case 5:
{ {
topicPubMsg[12] = "悬停模式"; topicPubMsg[12] = "悬停";
} }
break; break;
case 6: case 6:
{ {
topicPubMsg[12] = "返航模式"; topicPubMsg[12] = "返航";
} }
break; break;
case 9: case 9:
{ {
topicPubMsg[12] = "降落模式"; topicPubMsg[12] = "降落";
} }
break; break;
case 16: case 16:
{ {
topicPubMsg[12] = "定点模式"; topicPubMsg[12] = "定点";
} }
break; break;
default: default:
@ -1031,9 +1088,9 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
mavlink_msg_mission_request_decode(pMsg, &mission_request); // 解构msg数据 mavlink_msg_mission_request_decode(pMsg, &mission_request); // 解构msg数据
//日志 //日志
fc.logln("MsgID:"); fc.logln((char*)"MsgID:");
fc.logln(pMsg->msgid); fc.logln(pMsg->msgid);
fc.logln("No:"); fc.logln((char*)"No:");
fc.logln(mission_request.seq); fc.logln(mission_request.seq);
//飞控 反馈当前要写入航点的序号 //飞控 反馈当前要写入航点的序号
fc.writeSeq = mission_request.seq; fc.writeSeq = mission_request.seq;
@ -1045,9 +1102,9 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象 mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象
mavlink_msg_mission_ack_decode(pMsg, &mission_ark); // 解构msg数据 mavlink_msg_mission_ack_decode(pMsg, &mission_ark); // 解构msg数据
/*日志*/ /*日志*/
fc.logln("MsgID:"); fc.logln((char*)"MsgID:");
fc.logln(pMsg->msgid); fc.logln(pMsg->msgid);
fc.logln("re:"); fc.logln((char*)"re:");
fc.logln(mission_ark.type); fc.logln(mission_ark.type);
/*记录航点的写入状态 */ /*记录航点的写入状态 */
fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败 fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败

38
src/motocontrol.cpp
View File

@ -49,6 +49,44 @@ void Motocontrol::setweight(int pullweight) // 设置重量
_pullweight = pullweight; _pullweight = pullweight;
checkgoods(); checkgoods();
} }
String Motocontrol::gethooktatus_str()
{
String hookstatusstr="未知";
switch (_hooksstatus)
{
case HS_UnInit:
hookstatusstr="未初始化";
break;
case HS_Down:
hookstatusstr="放钩";
break;
case HS_DownSlow:
hookstatusstr="慢速放钩";
break;
case HS_WaitUnhook:
hookstatusstr="等待脱钩";
break;
case HS_Up:
hookstatusstr="回收";
break;
case HS_InStore:
hookstatusstr="入仓中";
break;
case HS_Locked:
hookstatusstr="已锁定";
break;
case HS_Stop:
hookstatusstr="停止";
break;
default:
hookstatusstr="未知";
break;
}
return hookstatusstr;
}
void Motocontrol::checkgoods() // 检测是否超重 void Motocontrol::checkgoods() // 检测是否超重
{ {

2
src/motocontrol.h
View File

@ -130,5 +130,7 @@ public:
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收 void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
void moto_goodsdownresume(); // 恢复自动放线 void moto_goodsdownresume(); // 恢复自动放线
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
String gethooktatus_str(); // 得到挂钩状态
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
}; };