串口接收调整，20hz接收测试正常,超长度解析正常

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
 

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;
         

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;
-    // }
+
 }