From 99dd5fb36d248c47f23e04dad69a61854086ce6d Mon Sep 17 00:00:00 2001 From: "Brown.Z" Date: Thu, 31 Mar 2022 14:07:37 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=B2=E5=8F=A3=E6=8E=A5=E6=94=B6=E8=B0=83?= =?UTF-8?q?=E6=95=B4=EF=BC=8C20hz=E6=8E=A5=E6=94=B6=E6=B5=8B=E8=AF=95?= =?UTF-8?q?=E6=AD=A3=E5=B8=B8,=E8=B6=85=E9=95=BF=E5=BA=A6=E8=A7=A3?= =?UTF-8?q?=E6=9E=90=E6=AD=A3=E5=B8=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/UserCode.cpp | 2 +- ArduCopter/zr_app.cpp | 21 +++---- libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp | 75 +++++++++++------------- 3 files changed, 47 insertions(+), 51 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 8ff56fa648..23e07c6c39 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -12,6 +12,7 @@ void Copter::userhook_init() void Copter::userhook_FastLoop() { // put your 100Hz code here + zr_app_50hz(); } #endif @@ -27,7 +28,6 @@ void Copter::userhook_MediumLoop() { // put your 10Hz code here zr_app_10hz(); - zr_app_50hz(); } #endif diff --git a/ArduCopter/zr_app.cpp b/ArduCopter/zr_app.cpp index 756dad2946..fe8fc10633 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -3,6 +3,16 @@ void Copter::zr_app_10hz() { + + if(zr_serial_api.data_trans_init){ +// 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) + uint16_t now_volt = uint16_t(battery.voltage() * 100); + zr_serial_api.get_vehicle_status((uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct()); + } + +} +void Copter::zr_app_50hz(){ + if(zr_serial_api.data_trans_init){ zr_serial_api.update(); @@ -15,16 +25,7 @@ void Copter::zr_app_10hz() gcs().send_text(MAV_SEVERITY_INFO, "data_trans init"); } } - - if(zr_serial_api.data_trans_init){ -// 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) - uint16_t now_volt = uint16_t(battery.voltage() * 100); - zr_serial_api.get_vehicle_status((uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing(),home_distance(),now_volt,battery.capacity_remaining_pct()); - } - -} -void Copter::zr_app_50hz(){ - + if(zr_serial_api.data_trans_init){ Vector3l pos_neu_cm; diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index 2c5b678836..9f0bf46eb8 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp @@ -57,12 +57,8 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager) // 接收主设备的串口数据 if(_zr_api_port == nullptr ) _zr_api_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_ZR_Serial_API, 0); - - // uint32_t baudrate = 460800U; bool init_host; if(_zr_api_port != nullptr ){ - // _zr_api_port->begin(baudrate); - // _zr_api_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); init_host = true; }else{ init_host = false; @@ -107,23 +103,39 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da } WITH_SEMAPHORE(sem); uint32_t nbytes = _port->available(); + // if(nbytes > 0){ - // serial_last_data_time = AP_HAL::millis(); - // } - // while(nbytes-->0) - // { - // // uint32_t time_decode = AP_HAL::millis(); - // uint8_t temp = _port->read(); - // data[*len] = temp; - // *len += 1; + // if (*len < bufsize) { // 判断数据是否存满 + // serial_last_data_time = AP_HAL::millis(); + // ssize_t nread = _port->read(pktbuf, MIN(nbytes, unsigned(CONTROL_DATA_LEN))); + // if (nread > 0) { + // *len = nread; + // memcpy(data,pktbuf,*len); // 收到一帧数据 + // } + // Debug("read:%ld,len:%d",nbytes,*len); + // }else{ + // Debug("out size:%d",*len); + // *len = 0; + // } // } - + if(nbytes > 0){ - serial_last_data_time = AP_HAL::millis(); - ssize_t nread = _port->read(pktbuf, MIN(nbytes, unsigned(CONTROL_DATA_LEN))); - if (nread > 0) { - *len = nread; - memcpy(data,pktbuf,*len); // 收到一帧数据 + uint8_t max_len = unsigned(CONTROL_DATA_LEN); + if (*len < max_len) { // 判断数据是否存满 + uint16_t count = MIN(nbytes, unsigned(max_len-*len)); + // Debug("t:%ld,read:%ld,len:%d,cnt:%d",AP_HAL::millis(),nbytes,*len,count); + while (count--) { + const int16_t temp = _port->read(); + if (temp == -1) { + break; + } + data[*len] = (uint8_t)temp; + *len+=1; + } + serial_last_data_time = AP_HAL::millis(); + }else{ + // Debug("t:%ld,out size:%d",AP_HAL::millis(),*len); + *len = 0; } } @@ -132,6 +144,7 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da check the UART for more data returns true if the function should be called again straight away */ +//TODO: 可能引发数组越界，先不用这套串口检查 bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port) { if (_port == nullptr) { @@ -145,6 +158,7 @@ bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port) if (n == 0) { return false; } + // Debug("t:%ld,read:%ld,len:%d",AP_HAL::millis(),n,pktoffset); if (pktoffset < bufsize) { // 判断数据是否存满 ssize_t nread = _port->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset))); // 限制读取长度，收到的和剩余空间长度取小的值 serial_last_data_time = AP_HAL::millis(); @@ -154,12 +168,6 @@ bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port) 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); @@ -299,11 +307,7 @@ void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t h vehicle_status.msg.home_distance = home_distance; vehicle_status.msg.volt_mv = volt_mv; vehicle_status.msg.bat_remaining = bat_remaining; - - // for (size_t i = 0; i < VEHICLE_STATUS_LEN; i++) - // { - // crc_sum += vehicle_status.data[i]; - // } + crc_sum = crc_crc8(vehicle_status.data,VEHICLE_STATUS_LEN-1); vehicle_status.msg.crc = crc_sum; write_data_to_port(_zr_api_port,vehicle_status.data, VEHICLE_STATUS_LEN); @@ -326,11 +330,6 @@ void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3 flight_data_parser.msg.msg_type = ZR_Msg_Type::MSG_FLIGHT_DATA; flight_data_parser.msg.length = FLIGHT_DATA_LEN - 4; - // flight_data_parser.msg.time_ms = AP_HAL::millis(); - - // flight_data_parser.msg.fly_mode = mode; - // flight_data_parser.msg.fly_status = in_air; - flight_data_parser.msg.lat = pos.x; flight_data_parser.msg.lng = pos.y; flight_data_parser.msg.alt = pos.z; @@ -435,7 +434,7 @@ void AC_ZR_Serial_API::update(void) 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)){ // 接收可能被中断，延时一段 + if((serial_data_len > 0 && (now_time - serial_last_data_time > 50)) || serial_data_len >= CONTROL_DATA_LEN){ // 接收可能被中断，延时一段 // Debug("%d,receive len:%d",now_time,serial_data_len); if(serial_data_len == CONTROL_DATA_LEN){ // 正常控制指令都是 CONTROL_DATA_LEN 长度 WITH_SEMAPHORE(sem); @@ -446,11 +445,7 @@ void AC_ZR_Serial_API::update(void) // 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); - // get_mav_data = false; - // } + }