From dfa58050c639aa208495322ff2848728fc6bf24d Mon Sep 17 00:00:00 2001 From: zbr3550 Date: Tue, 22 Mar 2022 16:03:53 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=8E=A7=E5=88=B6=E6=8C=87?= =?UTF-8?q?=E4=BB=A4=E5=92=8C=E5=BA=94=E7=AD=94=E7=9A=84=E6=B6=88=E6=81=AF?= =?UTF-8?q?ID?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp | 31 ++++++++++++++---------- libraries/AC_ZR_APP/AC_ZR_Serial_API.h | 21 +++++++++------- 2 files changed, 30 insertions(+), 22 deletions(-) diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index bf80924f2f..0ba5cfa4da 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp @@ -211,13 +211,14 @@ void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3 /** * @brief 获取串口接收到的控制数据 * + * @param mode 飞行模式，如果不是guide则false * @param type 控制模式，速度模式，位置模式 * @param data 控制的数据，根据type决定是速度还是位置 * @param yaw_deg 偏航角 * @return true * @return false */ -bool AC_ZR_Serial_API::get_control_data(ZR_Msg_Type &type,Vector3l &data,float &yaw_deg) +bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l &data,float &yaw_deg) { static uint32_t last_time = 0; uint8_t crc_sum = 0; @@ -235,22 +236,30 @@ bool AC_ZR_Serial_API::get_control_data(ZR_Msg_Type &type,Vector3l &data,float & if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){ Debug("crc error clc: %02x ,rece: %02x",crc_sum,flight_control_parser.msg.crc); - set_control_ask(flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误 + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误 return false; // 校验失败 } type = flight_control_parser.msg.type; if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL ){ - set_control_ask(flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答CRC错误 + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误 return false; // 控制类型错误 } - set_control_ask(flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功 - // 速度或者位置，3个float - memcpy(&data,flight_control_parser.data + 3,12); // 第三位开始是控制数据，长度3*4 + if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){ + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); + return false; // 飞行模式错误 + } + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功 + // 速度或者位置，3个int32 + // memcpy(&data,flight_control_parser.data + 4,12); // 第4位开始是控制数据，长度3*4 + data.x = flight_control_parser.msg.x; + data.y = flight_control_parser.msg.y; + data.z = flight_control_parser.msg.z; yaw_deg = flight_control_parser.msg.yaw; return true; } -void AC_ZR_Serial_API::set_control_ask(ZR_Msg_Type receive_type,ZR_Msg_ASK ask) + +void AC_ZR_Serial_API::set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask) { uint8_t crc_sum = 0; @@ -258,14 +267,10 @@ void AC_ZR_Serial_API::set_control_ask(ZR_Msg_Type receive_type,ZR_Msg_ASK ask) flight_control_ask.msg.msg_type = ZR_Msg_Type::MSG_ASK; flight_control_ask.msg.length = CONTROL_ASK_LEN - 4; - + flight_control_ask.msg.msg_id = msg_id; flight_control_ask.msg.receive_type = receive_type; flight_control_ask.msg.msg_ask = ask; - // crc_clc += flight_control_ask.msg.msg_head; - // crc_clc += (int)flight_control_ask.msg.msg_type; - // crc_clc += (int)flight_control_ask.msg.receive_type; - // crc_clc += (int)flight_control_ask.msg.msg_ask; - + crc_sum = crc_crc8(flight_control_ask.data,CONTROL_ASK_LEN-1); flight_control_ask.msg.crc = crc_sum; diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index d92e764b1d..6e7b580e53 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h @@ -22,9 +22,9 @@ #define MSG_HEAD 0xFE #define FLIGHT_DATA_LEN 40 -#define CONTROL_DATA_LEN 20 +#define CONTROL_DATA_LEN 21 #define VEHICLE_STATUS_LEN 13 -#define CONTROL_ASK_LEN 6 +#define CONTROL_ASK_LEN 7 class AC_ZR_Serial_API { @@ -54,6 +54,7 @@ public: MSG_ASK_OK = 0, // 成功 MSG_ASK_ERRCRC , // crc校验错误 MSG_ASK_ERRTYPE , // 消息类型错误 + MSG_ASK_ERRMODE , // 飞行模式错误 MSG_ASK_OUTRANGE // 控制数据超出限制 }; @@ -72,9 +73,9 @@ public: void get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining); - bool get_control_data(ZR_Msg_Type &type,Vector3l &data,float &yaw_deg); - - void set_control_ask(ZR_Msg_Type receive_type,ZR_Msg_ASK ask); + bool get_control_data(uint8_t mode,ZR_Msg_Type &type,Vector3l &data,float &yaw_deg); + // 接收控制指令的应答 + void set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask); bool data_trans_init; @@ -125,13 +126,14 @@ private: uint8_t head; ZR_Msg_Type type; uint8_t length; - // 3 + uint8_t msg_id; + // 4 int32_t x; int32_t y; int32_t z; - // 15 + // 16 float yaw; - // 19 + // 10 uint8_t crc; }; union PACKED flight_control_parser_u @@ -145,7 +147,8 @@ private: uint8_t msg_head; ZR_Msg_Type msg_type; uint8_t length; - // 3 + uint8_t msg_id; + // 4 ZR_Msg_Type receive_type; ZR_Msg_ASK msg_ask; uint8_t crc;