diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 02a0542..1a0483d 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -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(result); // 低位 互换 + bytes[len + 1] = static_cast(result >> 8); // 高位 互换 + //udp发送命令帧 + udp.beginPacket(udpServerIP, udpServerPort); + udp.write(bytes, len + 2); + udp.endPacket(); +} + /** * @description: 按位设置 1 or 0 * @param {String} str 要设置的值 diff --git a/src/FoodDeliveryBase.h b/src/FoodDeliveryBase.h index 5924d0d..93ec278 100644 --- a/src/FoodDeliveryBase.h +++ b/src/FoodDeliveryBase.h @@ -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 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index ce73a21..c8fb942 100644 --- a/src/main.cpp +++ b/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(); + + //相机控制 + 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表示失败 diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 4f72ce6..41e4f4a 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -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() // 检测是否超重 { diff --git a/src/motocontrol.h b/src/motocontrol.h index fa78bd1..e704d10 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -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 };