1.合并小田新的文件,加入相机云台控制
2.修改钩子控制部分 3.反馈给服务器钩子文字状态
This commit is contained in:
parent
2ffb029774
commit
96df77d785
@ -3,7 +3,7 @@
|
||||
/**
|
||||
* @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帐号
|
||||
password = userPassword; //wifi密码
|
||||
@ -13,6 +13,8 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
|
||||
mqttPassword = userMqttPassword; //mqtt密码
|
||||
mavlinkSerial = userMavlinkSerial; //mavlink用的串口
|
||||
voiceSerial = userVoiceSerial; //声音模块用的串口
|
||||
udpServerIP = userUdpServerIP; //云台相机ip
|
||||
udpServerPort = userUdpServerPort; //云台相机端口
|
||||
|
||||
//初始化飞控通讯串口 波特率
|
||||
switch (mavlinkSerial) { //初始化指定 串口号
|
||||
@ -113,6 +115,7 @@ void FoodCube::connectWifi() {
|
||||
macAdd.replace(":", ""); //板子的物理地址 并且去除冒号
|
||||
log("macAdd: ");
|
||||
logln(macAdd);
|
||||
playText("歪佛哎,已连接");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
@ -134,6 +137,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
|
||||
for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题
|
||||
subscribeTopic(topicSub[i], 1);
|
||||
}
|
||||
playText("哎木可优踢踢,已连接");
|
||||
} else {
|
||||
//失败返回状态码
|
||||
log("MQTT Server Connect Failed. Client State:");
|
||||
@ -207,6 +211,7 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @description: 声音播放
|
||||
* @param {String} str 播放内容
|
||||
@ -286,6 +291,52 @@ void FoodCube::stopVoice() {
|
||||
SWrite(stop, sizeof(stop), voiceSerial); //发送检查指令
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 相机控制命令帧 校检码
|
||||
* @param {uint8_t[]} 命令帧 数组
|
||||
* @param {int} len 命令帧 的长度
|
||||
*/
|
||||
uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) {
|
||||
uint16_t crc, oldcrc16;
|
||||
uint8_t temp;
|
||||
crc = crc_init;
|
||||
while (len-- != 0) {
|
||||
temp = (crc >> 8) & 0xff;
|
||||
oldcrc16 = crc16_tab[*ptr ^ temp];
|
||||
crc = (crc << 8) ^ oldcrc16;
|
||||
ptr++;
|
||||
}
|
||||
return (crc);
|
||||
}
|
||||
|
||||
void FoodCube::crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result) {
|
||||
uint16_t crc_result = 0;
|
||||
crc_result = CRC16_cal(pbuf, len, 0);
|
||||
*p_result = crc_result;
|
||||
}
|
||||
/**
|
||||
* @description: 相机
|
||||
* @param {uint8_t[]} 命令帧 数组
|
||||
* @param {int} len 命令帧 的长度
|
||||
*/
|
||||
void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) {
|
||||
uint16_t result = 0;
|
||||
uint16_t* p_result = &result;
|
||||
//计算校检码
|
||||
crc_check_16bites(p_command, len, p_result);
|
||||
//加上校检码
|
||||
uint8_t bytes[2 + len];
|
||||
for (int i = 0; i < len; i++) {
|
||||
bytes[i] = p_command[i];
|
||||
}
|
||||
bytes[len] = static_cast<uint8_t>(result); // 低位 互换
|
||||
bytes[len + 1] = static_cast<uint8_t>(result >> 8); // 高位 互换
|
||||
//udp发送命令帧
|
||||
udp.beginPacket(udpServerIP, udpServerPort);
|
||||
udp.write(bytes, len + 2);
|
||||
udp.endPacket();
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 按位设置 1 or 0
|
||||
* @param {String} str 要设置的值
|
||||
|
||||
@ -11,6 +11,8 @@
|
||||
#include "ArduinoJson.h"
|
||||
/*异步库*/
|
||||
#include "Ticker.h"
|
||||
/*udp发送*/
|
||||
#include "WiFiUdp.h"
|
||||
|
||||
class FoodCube {
|
||||
public:
|
||||
@ -22,7 +24,7 @@ public:
|
||||
/*前端模拟遥控的油门通道*/
|
||||
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(char* val);
|
||||
@ -47,7 +49,7 @@ public:
|
||||
void subscribeTopic(String topicString, int Qos);
|
||||
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);
|
||||
uint8_t chekVoiceMcu();
|
||||
@ -61,6 +63,8 @@ public:
|
||||
void mav_set_mode(uint8_t SysState);
|
||||
void mav_command(uint8_t controlType, uint16_t param[]);
|
||||
void mav_channels_override(uint16_t chan[]);
|
||||
/*云台相机控制*/
|
||||
void udpSendToCamera(uint8_t* p_command, uint32_t len);
|
||||
|
||||
private:
|
||||
char* ssid; //wifi帐号
|
||||
@ -70,11 +74,53 @@ private:
|
||||
char* mqttName; //mqtt帐号
|
||||
char* mqttPassword; //mqtt密码
|
||||
uint8_t mavlinkSerial; //飞控占用的串口号
|
||||
uint8_t voiceSerial; //飞控占用的串口号
|
||||
uint8_t voiceSerial; //飞控占用的串口号
|
||||
bool isInit = true; //用来判断接收到第一次心跳时 重新向飞控 发送一个请求数据类型
|
||||
|
||||
WiFiClient wifiClient; //网络客户端
|
||||
IPAddress localIp; //板子的IP地址
|
||||
String macAdd; //板子的物理地址(已去掉冒号分隔符)
|
||||
|
||||
/*云台相机控制*/
|
||||
WiFiUDP udp; //udp信息操作对象
|
||||
char* udpServerIP; //云台相机ip地址
|
||||
uint32_t udpServerPort; //云台相机端口
|
||||
//摄像头控制 校验代码
|
||||
const uint16_t crc16_tab[256] = { 0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
|
||||
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
|
||||
0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
|
||||
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
|
||||
0x2462, 0x3443, 0x420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
|
||||
0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
|
||||
0x3653, 0x2672, 0x1611, 0x630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
|
||||
0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
|
||||
0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840, 0x1861, 0x2802, 0x3823,
|
||||
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
|
||||
0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50, 0x3a33, 0x2a12,
|
||||
0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
|
||||
0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0xc60, 0x1c41,
|
||||
0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
|
||||
0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0xe70,
|
||||
0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
|
||||
0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
|
||||
0x1080, 0xa1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
|
||||
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
|
||||
0x2b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
|
||||
0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
|
||||
0x34e2, 0x24c3, 0x14a0, 0x481, 0x7466, 0x6447, 0x5424, 0x4405,
|
||||
0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
|
||||
0x26d3, 0x36f2, 0x691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
|
||||
0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
|
||||
0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1, 0x3882, 0x28a3,
|
||||
0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
|
||||
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1, 0x1ad0, 0x2ab3, 0x3a92,
|
||||
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
|
||||
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,
|
||||
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
|
||||
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0 };
|
||||
uint16_t CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init);
|
||||
void crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
133
src/main.cpp
133
src/main.cpp
@ -56,7 +56,7 @@ void Task1(void *pvParameters);
|
||||
void led_show(CRGB ledcolor);
|
||||
// void Task1code( void *pvParameters );
|
||||
void showledidel();
|
||||
int pullweight = 0;
|
||||
int pullweight = 0; //检测重量-克
|
||||
int8_t btn_pressatonce=0; //是否同时按下
|
||||
unsigned long _tm_bengstop;
|
||||
bool _bengstop = false;
|
||||
@ -88,32 +88,35 @@ unsigned long _tm_waitinit;
|
||||
/*项目对象*/
|
||||
//char* ssid = "szdot"; //wifi帐号
|
||||
//char* password = "Ttaj@#*.com"; //wifi密码
|
||||
char* ssid = "flicube"; //wifi帐号
|
||||
char* password = "fxmf0622"; //wifi密码
|
||||
char* mqttServer = "szdot.top"; //mqtt地址
|
||||
char* ssid = (char*)"flicube"; //wifi帐号
|
||||
char* password = (char*)"fxmf0622"; //wifi密码
|
||||
char* mqttServer = (char*)"szdot.top"; //mqtt地址
|
||||
int mqttPort = 1883; //mqtt端口
|
||||
char* mqttName = "admin"; //mqtt帐号
|
||||
char* mqttPassword = "123456"; //mqtt密码
|
||||
char* mqttName = (char*)"admin"; //mqtt帐号
|
||||
char* mqttPassword = (char*)"123456"; //mqtt密码
|
||||
uint8_t mavlinkSerial = 2; //飞控占用的串口号
|
||||
uint8_t voiceSerial = 1; //声音模块占用串口
|
||||
char* udpServerIP = (char*) "192.168.8.100"; //云台相机ip
|
||||
uint32_t udpServerPort = 37260; //云台相机端口
|
||||
|
||||
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
|
||||
void mavThread();
|
||||
void pubThread();
|
||||
void flashThread() ;
|
||||
void writeRoute(String topicStr);
|
||||
void mavlink_receiveCallback(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
|
||||
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4" }; //订阅主题
|
||||
//0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 8:钩子控制
|
||||
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" }; //订阅主题
|
||||
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数
|
||||
/*有更新主动发送 主题*/
|
||||
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式
|
||||
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode" };
|
||||
//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","loadweight","hookstatus" };
|
||||
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数
|
||||
String topicPubMsg[13]; //发送数据存放 对应topicPub
|
||||
String oldMsg[13]; //记录旧的数据 用来对比有没有更新
|
||||
String topicPubMsg[15]; //发送数据存放 对应topicPub
|
||||
String oldMsg[15]; //记录旧的数据 用来对比有没有更新
|
||||
/*触发发送 主题*/
|
||||
//0:对频信息
|
||||
String topicHandle[] = { "crosFrequency" };
|
||||
@ -215,7 +218,9 @@ void showinfo()
|
||||
// if (pullweight > 10)
|
||||
// printf("PullWeight:%d\n", pullweight);
|
||||
|
||||
// HookStatus hs=motocontrol.gethooktatus() ;
|
||||
topicPubMsg[14]=motocontrol.gethooktatus_str() ;
|
||||
topicPubMsg[13]=pullweight;
|
||||
|
||||
// control_status_t cs=motocontrol.getcontrolstatus() ;
|
||||
|
||||
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
|
||||
@ -351,7 +356,6 @@ void loop()
|
||||
}
|
||||
// 检测执行初始化工作
|
||||
checkinited();
|
||||
|
||||
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
||||
/*从飞控拿数据*/
|
||||
fc.comm_receive(mavlink_receiveCallback);
|
||||
@ -377,6 +381,8 @@ void Task1(void *pvParameters)
|
||||
scale.tare();
|
||||
}
|
||||
pullweight = get_pullweight();
|
||||
// printf("pullweight: %d \n", pullweight);
|
||||
|
||||
vTaskDelay(10);
|
||||
}
|
||||
}
|
||||
@ -657,7 +663,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
||||
/*订阅回调主体*/
|
||||
if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss
|
||||
writeRoute(topicStr); //写入航点
|
||||
}
|
||||
} else
|
||||
if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState
|
||||
/*
|
||||
*其中topicPubMsg[10]既飞机状态的值
|
||||
@ -721,29 +727,74 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
||||
}
|
||||
if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState
|
||||
fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态
|
||||
}
|
||||
}else
|
||||
if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState
|
||||
topicPubMsg[10] = "1"; //恢复初始状态
|
||||
}
|
||||
}else
|
||||
if (cutTopic == topicSub[4]) { //4:油门通道1
|
||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
||||
fc.channels[0] = strInt; //恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); //油门控制
|
||||
}
|
||||
}else
|
||||
if (cutTopic == topicSub[5]) { //5:油门通道2
|
||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
||||
fc.channels[1] = strInt; //恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); //油门控制
|
||||
}
|
||||
}else
|
||||
if (cutTopic == topicSub[6]) { //6:油门通道3
|
||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
||||
fc.channels[2] = strInt; //恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); //油门控制
|
||||
}
|
||||
}else
|
||||
if (cutTopic == topicSub[7]) { //7:油门通道4
|
||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //强制转换一下
|
||||
fc.channels[3] = strInt; //恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); //油门控制
|
||||
}else
|
||||
if (cutTopic == topicSub[8]) { //8:钩子控制
|
||||
uint16_t strInt = (uint16_t)topicStr.toInt(); //
|
||||
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) {
|
||||
if (fc.writeState) // 如果正在写入状态 跳出
|
||||
{
|
||||
fc.logln("正在写航点"); // 提示正在写入中
|
||||
fc.logln((char*)"正在写航点"); // 提示正在写入中
|
||||
return;
|
||||
}
|
||||
//改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
|
||||
@ -775,7 +826,7 @@ void writeRoute(String topicStr) {
|
||||
fc.comm_receive(mavlink_receiveCallback);
|
||||
if (fc.missionArkType == 0) { //写入成功
|
||||
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], 1, 0); //结束写入状态
|
||||
@ -783,7 +834,7 @@ void writeRoute(String topicStr) {
|
||||
break;
|
||||
} else if (fc.missionArkType > 0) { //写入失败
|
||||
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], 1, 0); //结束写入状态
|
||||
@ -804,7 +855,7 @@ void writeRoute(String topicStr) {
|
||||
double x = obj["tasks"][fc.writeSeq]["x"];
|
||||
double y = obj["tasks"][fc.writeSeq]["y"];
|
||||
double z = obj["tasks"][fc.writeSeq]["z"];
|
||||
fc.logln("frame--");
|
||||
fc.logln((char*)"frame--");
|
||||
fc.logln(frame);
|
||||
//写入航点
|
||||
fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
|
||||
@ -835,13 +886,19 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
//从心跳里判断 飞机是否解锁 解锁改变飞机状态
|
||||
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁
|
||||
if (!curr_armed)
|
||||
{
|
||||
printf("armed\n");
|
||||
fc.playText("已解锁");
|
||||
}
|
||||
curr_armed=true;
|
||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
|
||||
} else {
|
||||
if (curr_armed)
|
||||
{
|
||||
printf("disarm\n");
|
||||
fc.playText("已加锁");
|
||||
curr_armed=false; //心跳里面 判断没有解锁
|
||||
}
|
||||
curr_armed=false; //心跳里面 判断没有解锁
|
||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
|
||||
if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点
|
||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态
|
||||
@ -857,42 +914,42 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
switch (heartbeat.custom_mode) {
|
||||
case 0:
|
||||
{
|
||||
topicPubMsg[12] = "姿态模式";
|
||||
topicPubMsg[12] = "姿态";
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
topicPubMsg[12] = "定高模式";
|
||||
topicPubMsg[12] = "定高";
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
{
|
||||
topicPubMsg[12] = "自动模式";
|
||||
topicPubMsg[12] = "自动";
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
{
|
||||
topicPubMsg[12] = "引导模式";
|
||||
topicPubMsg[12] = "指点";
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
{
|
||||
topicPubMsg[12] = "悬停模式";
|
||||
topicPubMsg[12] = "悬停";
|
||||
}
|
||||
break;
|
||||
case 6:
|
||||
{
|
||||
topicPubMsg[12] = "返航模式";
|
||||
topicPubMsg[12] = "返航";
|
||||
}
|
||||
break;
|
||||
case 9:
|
||||
{
|
||||
topicPubMsg[12] = "降落模式";
|
||||
topicPubMsg[12] = "降落";
|
||||
}
|
||||
break;
|
||||
case 16:
|
||||
{
|
||||
topicPubMsg[12] = "定点模式";
|
||||
topicPubMsg[12] = "定点";
|
||||
}
|
||||
break;
|
||||
default:
|
||||
@ -1031,9 +1088,9 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
|
||||
mavlink_msg_mission_request_decode(pMsg, &mission_request); // 解构msg数据
|
||||
//日志
|
||||
fc.logln("MsgID:");
|
||||
fc.logln((char*)"MsgID:");
|
||||
fc.logln(pMsg->msgid);
|
||||
fc.logln("No:");
|
||||
fc.logln((char*)"No:");
|
||||
fc.logln(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_msg_mission_ack_decode(pMsg, &mission_ark); // 解构msg数据
|
||||
/*日志*/
|
||||
fc.logln("MsgID:");
|
||||
fc.logln((char*)"MsgID:");
|
||||
fc.logln(pMsg->msgid);
|
||||
fc.logln("re:");
|
||||
fc.logln((char*)"re:");
|
||||
fc.logln(mission_ark.type);
|
||||
/*记录航点的写入状态 */
|
||||
fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败
|
||||
|
||||
@ -49,6 +49,44 @@ void Motocontrol::setweight(int pullweight) // 设置重量
|
||||
_pullweight = pullweight;
|
||||
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() // 检测是否超重
|
||||
{
|
||||
|
||||
@ -130,5 +130,7 @@ public:
|
||||
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
|
||||
void moto_goodsdownresume(); // 恢复自动放线
|
||||
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
|
||||
String gethooktatus_str(); // 得到挂钩状态
|
||||
|
||||
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user