From 2d67228afe91e758234ef01cd986331a734d8c26 Mon Sep 17 00:00:00 2001 From: "Brown.Z" Date: Tue, 29 Mar 2022 17:58:48 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=B2=E5=8F=A3=E8=AF=BB=E5=8F=96=E8=B0=83?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/zr_app.cpp | 4 +- just_build.sh | 2 +- libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp | 136 ++++++++++++++++++++++- libraries/AC_ZR_APP/AC_ZR_Serial_API.h | 16 +++ zr-v5.sh | 3 +- 5 files changed, 155 insertions(+), 6 deletions(-) diff --git a/ArduCopter/zr_app.cpp b/ArduCopter/zr_app.cpp index a6097232e0..7e9ae165f7 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -64,7 +64,7 @@ void Copter::zr_app_50hz(){ float yaw_deg; bool new_data = zr_serial_api.get_control_data((uint8_t)flightmode->mode_number() ,msg_type,data,yaw_deg); if(new_data){ - gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%d,%d,%d, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg); + gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg); switch (msg_type) { case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF: @@ -89,7 +89,7 @@ void Copter::zr_app_50hz(){ target_loc.alt = (data.z + 500); // target_loc.alt = (data.z + 500)/100.0; set_target_location(target_loc); - gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %d,%d,%d",target_loc.lat,target_loc.lng,target_loc.alt); + gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt); } break; case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL: diff --git a/just_build.sh b/just_build.sh index 8427907ff7..ff85600a38 100755 --- a/just_build.sh +++ b/just_build.sh @@ -1,2 +1,2 @@ ./waf copter -cp build/Pixhawk4/bin/arducopter.apj /mnt/hgfs/firmware/编译/zr-415.px4 \ No newline at end of file +cp build/zr-v5/bin/arducopter.apj /mnt/hgfs/firmware/编译/zrv5-415.px4 \ No newline at end of file diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index 0ba5cfa4da..8d83d13652 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp @@ -41,6 +41,12 @@ AC_ZR_Serial_API::AC_ZR_Serial_API() if (_singleton != nullptr) { AP_HAL::panic("AC_ZR_Serial_API must be singleton"); } + bufsize = MAX(CONTROL_DATA_LEN, 50); + pktbuf = new uint8_t[bufsize]; + if (!pktbuf ) { + AP_HAL::panic("Failed to allocate AC_ZR_Serial_API"); + } + _singleton = this; } @@ -111,7 +117,129 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da *len += 1; } } +/* + check the UART for more data + returns true if the function should be called again straight away + */ +bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port) +{ + if (_port == nullptr) { + return false; + } + uint16_t crc; + WITH_SEMAPHORE(control_data.sem); + + uint32_t n = _port->available(); + if (n == 0) { + return false; + } + if (pktoffset < bufsize) { + ssize_t nread = _port->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset))); + serial_last_data_time = AP_HAL::millis(); + if (nread <= 0) { + return false; + } + pktoffset += nread; + } + + if (pktbuf[0] != 0xFE) { + // goto reset; + } + + + + uint8_t *p = (uint8_t *)memchr(&pktbuf[0], (char)0xFE, pktoffset-1); + if (p) { + uint8_t newlen = pktoffset - (p - pktbuf); + memmove(&pktbuf[0], p, newlen); + pktoffset = newlen; + Debug("move msg:%d",newlen); + } else { + // pktoffset = 0; + goto reset; + } + + if (pktbuf[0] != 0xFE) { + Debug("no head:%d",pktoffset); + return false; + } + + crc = crc_crc8(pktbuf, CONTROL_DATA_LEN-1); + if (crc == pktbuf[CONTROL_DATA_LEN-1]) { + // got pkt1 + memcpy(flight_control_parser.data,pktbuf,CONTROL_DATA_LEN); // 收到一帧数据 + control_data_last_time = AP_HAL::millis(); + Debug(" %ld-get msg,ofs:%d ",control_data_last_time,pktoffset); + // process_packet(pktbuf); + memmove(&pktbuf[0], &pktbuf[CONTROL_DATA_LEN], pktoffset-CONTROL_DATA_LEN); + pktoffset -= CONTROL_DATA_LEN; + } else { + + Debug("crc error clc: %02x ,rece: %02x",crc,pktbuf[CONTROL_DATA_LEN-1]); + set_control_ask(pktbuf[3],flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误 + goto reset; + } + + return true; + +reset: + uint8_t *p2 = (uint8_t *)memchr(&pktbuf[1], (char)0xFE, pktoffset-1); + if (p2) { + uint8_t newlen = pktoffset - (p2 - pktbuf); + memmove(&pktbuf[0], p2, newlen); + pktoffset = newlen; + Debug("move msg:%d",newlen); + } else { + pktoffset = 0; + } + return true; +} + + +void AC_ZR_Serial_API::process_packet(const uint8_t *b) +{ + static uint32_t last_time = 0; + uint8_t crc_sum = 0; + + WITH_SEMAPHORE(control_data.sem); + + memcpy(flight_control_parser.data,b,CONTROL_DATA_LEN); + + if(control_data_last_time == last_time){ // 数据没更新，直接退出 + return ; + } + last_time = control_data_last_time; + if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头 + Debug("head error: %02x",flight_control_parser.msg.head ); + return ; + } + + 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.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误 + return ; // 校验失败 + } + if(flight_control_parser.msg.type < ZR_Msg_Type::MSG_CONTROL_TKOFF || flight_control_parser.msg.type > ZR_Msg_Type::MSG_CONTROL_VEL ){ + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误 + return ; // 控制类型错误 + } + if(flight_control_parser.msg.type != ZR_Msg_Type::MSG_CONTROL_TKOFF && flight_mode != 4){ + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); + return ; // 飞行模式错误 + } + + control_data.time_stamp = AP_HAL::millis(); + control_data_last_time = AP_HAL::millis(); + control_data.type = flight_control_parser.msg.type; + control_data.data.x = flight_control_parser.msg.x; + control_data.data.y = flight_control_parser.msg.y; + control_data.data.z = flight_control_parser.msg.z; + control_data.yaw_deg = flight_control_parser.msg.yaw; + + set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功 + +} void AC_ZR_Serial_API::get_vehicle_NEU_pos(Vector3f vec_neu) { static char buf[50]; @@ -138,6 +266,7 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler) void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining) { + flight_mode = mode; static uint8_t delay_cnt; delay_cnt += 1; @@ -227,6 +356,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l return false; } last_time = control_data_last_time; + WITH_SEMAPHORE(control_data.sem); if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头 Debug("head error: %02x",flight_control_parser.msg.head ); return false; @@ -283,8 +413,10 @@ void AC_ZR_Serial_API::update(void) if(!data_trans_init || _zr_api_port == nullptr){ return; } + #if 1 + check_uart(_zr_api_port); + #else read_data_from_port(_zr_api_port,serial_data_from_device, &serial_data_len); // 读取主设备串口数据 - // Debug("receive host data:%d",mav_data_len); uint32_t now_time = AP_HAL::millis(); if(serial_data_len > 0 && (now_time - serial_last_data_time > 50)){ // 接收可能被中断，延时一段 @@ -297,7 +429,7 @@ void AC_ZR_Serial_API::update(void) serial_data_len = 0; // memset(serial_data_from_device,0,CONTROL_DATA_LEN); } - + #endif // if(get_mav_data){ // Debug("%d,host data len:%d",now_time,mav_data_len); // write_data_to_port(_zr_api_port,mav_data_from_host, mav_data_len); diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index 6e7b580e53..22b86e7a20 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h @@ -82,6 +82,9 @@ public: private: + bool check_uart(AP_HAL::UARTDriver *_port); + void process_packet(const uint8_t *b); + static AC_ZR_Serial_API *_singleton; AP_HAL::UARTDriver *_zr_api_port; uint32_t serial_last_data_time; @@ -182,6 +185,19 @@ private: uint8_t data[VEHICLE_STATUS_LEN]; }vehicle_status; + struct control_t { + HAL_Semaphore sem; + + uint32_t time_stamp; + ZR_Msg_Type type; + Vector3l data; + float yaw_deg; + } control_data; + + uint8_t *pktbuf; + uint16_t pktoffset; + uint16_t bufsize; + uint8_t flight_mode; }; \ No newline at end of file diff --git a/zr-v5.sh b/zr-v5.sh index 02f9f2e3d6..f4af63e978 100755 --- a/zr-v5.sh +++ b/zr-v5.sh @@ -1,2 +1,3 @@ +./waf configure --board zr-v5 ./waf copter -cp build/zr-v5/bin/arducopter.apj /mnt/hgfs/firmware/编译/zr-415-01.px4 \ No newline at end of file +cp build/zr-v5/bin/arducopter.apj /mnt/hgfs/firmware/编译/zr-v5-415.px4 \ No newline at end of file