void *SBUS_DEV1_TO_FDSBUS_thread(void *argc)
{
int sbus_fd = *(int *)argc;
uint16_t i, CrcRecv, CrcCal;
uint16_t num_data;
uint8_t step = 0, parsed = FALSE;
uint8_t sbus_rx[SBUS_TOTAL_MSG_LEN], sbus_tx[25], sbus_temp[25];
uint8_t sbus_data[SBUS_BUF_LEN] = {0};
uint8_t data = 0;
uint16_t sbus_dec[16];
struct timeval tCurTime = {0};
struct timeval tLastTime = {0};
uint32_t commonlen;
uint64_t timeout;
int ret;
//优先级
uint8_t DATA_SOURCE_DEV1 = 0;
uint8_t DATA_SOURCE_UDP = 1;
bool frames_parsed = FALSE, sbusLinked = FALSE;
uint32_t CRCerr;
uint32_t fail_safe_cnt;
// 添加变量记录上次使用的数据源
static uint8_t last_use_dev1 = 0;
static uint8_t last_use_udp = 0;
uint8_t last_valid_sbus_tx[25];
static BOOL has_valid_data = FALSE; // 新增标志位
uint8_t sbus_failsafe = FALSE;
printf(“SBUS_DEV1_TO_FDSBUS_thread***\n”); gettimeofday(&tLastTime, NULL); while (1) { //从401D获取sbus数据 pthread_mutex_lock(&sbus_dev1_rx_mutex); uint32_t sbus_dev1_bufferlen = QueueNData(sbus_dev1_tx_buff); pthread_mutex_unlock(&sbus_dev1_rx_mutex); //从光纤获取到的SBUS数据 pthread_mutex_lock(&sbus_udp_recv_mutex); uint32_t sbus_udp_bufferlen = QueueNData(sbus_udp_recv_buff); pthread_mutex_unlock(&sbus_udp_recv_mutex); uint8_t use_dev1 = (sbus_dev1_bufferlen > 0); uint8_t use_udp = (sbus_udp_bufferlen > 0) && !use_dev1; // 数据源切换时重置解析状态 if ((use_dev1 && !last_use_dev1) || (use_udp && !last_use_udp)) { step = SBUS_MSG_HEAD_A_IDX; memset(sbus_rx, 0, sizeof(sbus_rx)); // printf(“Data source switched, reset parser\n”); } // 更新上次数据源状态 last_use_dev1 = use_dev1; last_use_udp = use_udp; if (use_dev1 || use_udp) { // Read from the appropriate source uint32_t bufferlen = use_dev1 ? sbus_dev1_bufferlen : sbus_udp_bufferlen; while (bufferlen) { // printf(“start decode sbus!\n\r”); if (use_dev1) { pthread_mutex_lock(&sbus_dev1_rx_mutex); QueueRead(&data, sbus_dev1_tx_buff, sizeof(data)); pthread_mutex_unlock(&sbus_dev1_rx_mutex); } else if (use_udp) { // printf(“use_udp sbus sned to uart\n\r”); pthread_mutex_lock(&sbus_udp_recv_mutex); QueueRead(&data, sbus_udp_recv_buff, sizeof(data)); pthread_mutex_unlock(&sbus_udp_recv_mutex); } bufferlen–; if (step == SBUS_MSG_HEAD_A_IDX) { if (data == SBUS_MSG_HEAD_A) { sbus_rx[step] = data; step = SBUS_MSG_HEAD_B_IDX; } } else if (step == SBUS_MSG_HEAD_B_IDX) { if (data == SBUS_MSG_HEAD_B) { sbus_rx[step] = data; step = SBUS_MSG_LEN_IDX; } else { step = SBUS_MSG_HEAD_A_IDX; } } else if (step == SBUS_MSG_LEN_IDX) { if (data == SBUS_MSG_VALID_DATA_LEN) { sbus_rx[step] = data; step = SBUS_MSG_DATA_START_IDX; } else { step = SBUS_MSG_HEAD_A_IDX; } } else if ((step >= SBUS_MSG_DATA_START_IDX) && (step <= SBUS_MSG_CRC_LOW_IDX)) { sbus_rx[step] = data; if (step == SBUS_MSG_CRC_LOW_IDX) { // CRC Check CrcRecv = (sbus_rx[SBUS_MSG_CRC_HIGH_IDX] << 8) | sbus_rx[SBUS_MSG_CRC_LOW_IDX]; CrcCal = crc16_ccitt(&sbus_rx[SBUS_MSG_DATA_START_IDX], SBUS_MSG_VALID_DATA_LEN); step = SBUS_MSG_HEAD_A_IDX; if (CrcCal == CrcRecv) { parsed = TRUE; if (sbusLinked == FALSE) { sbusLinked = TRUE; } fail_safe_cnt = 0; sbus_failsafe = FALSE; } else { CRCerr++; printf(“CRCerr:%d \n\r”, CRCerr); } } else { step++; } } else { step = SBUS_MSG_HEAD_A_IDX; } } } // else // { // usleep(100); // } if (parsed) { memset(sbus_tx, 0x00, sizeof(sbus_tx)); memcpy(&sbus_tx[1], &sbus_rx[SBUS_MSG_DATA_START_IDX], SBUS_MSG_VALID_DATA_LEN); sbus_tx[0] = 0x0F; // parsed =FALSE; memcpy(last_valid_sbus_tx, sbus_tx, sizeof(sbus_tx)); has_valid_data = TRUE; } gettimeofday(&tCurTime, NULL); timeout = (tCurTime.tv_sec - tLastTime.tv_sec) * 1000 + (tCurTime.tv_usec - tLastTime.tv_usec) / 1000; if (timeout > 7) { if (parsed) { ret = UART_Write(sbus_fd, (const char *)sbus_tx, sizeof(sbus_tx)); if (ret < 0) { printf(“UART_Write faild\n\r”); } gettimeofday(&tLastTime, NULL); parsed = FALSE; } else { if (sbusLinked) { fail_safe_cnt++; if (fail_safe_cnt > 142) { sbus_failsafe = TRUE; struct timeval tv; gettimeofday(&tv, NULL); printf(“[%ld.%06ld] SBUS-fail safe CRCerr:%d \n\r”, tv.tv_sec, tv.tv_usec, CRCerr); // printf(“SBUS-fail safe CRCerr:%d \n\r”, CRCerr); } } } // bool sent = FALSE; // // 优先发送新解析到的数据 // if (parsed) // { // int tx_ret = UART_Write(sbus_fd, (const char *)sbus_tx, sizeof(sbus_tx)); // if (tx_ret < 0) // { // printf(“UART_Write sbus_tx faild\n\r”); // } // sent = TRUE; // parsed = FALSE; // } // // 其次:如果没有新数据,但还有历史有效帧,且未超时 → 重发 last frame // else if (has_valid_data && fail_safe_cnt < 142) // < 142 * 7ms ≈ 994ms // { // int last_ret = UART_Write(sbus_fd, (const char *)last_valid_sbus_tx, sizeof(last_valid_sbus_tx)); // if (last_ret < 0 ) // { // printf(“UART_Write last_valid_sbus_tx faild \n\r”); // } // sent = TRUE; // fail_safe_cnt++; // 计数器递增 // } // // 超过 1s 无有效输入 → 不再发送任何数据 // else // { // if (fail_safe_cnt == 142) // { // struct timeval tv; // gettimeofday(&tv, NULL); // printf(“[%ld.%06ld] SBUS TIMEOUT: No valid input for 1 second. Stopping output.\n”, // tv.tv_sec, tv.tv_usec); // sbusLinked = FALSE; // } // fail_safe_cnt++; // } // gettimeofday(&tLastTime, NULL); // // 可选:打印状态 // if (fail_safe_cnt > 0 && fail_safe_cnt % 50 == 0) // { // struct timeval tv; // gettimeofday(&tv, NULL); // printf(“[%ld.%06ld] SBUS fallback: %u cycles (%dms) since last valid frame\n”, // tv.tv_sec, tv.tv_usec, fail_safe_cnt, fail_safe_cnt * 7); // } // } // else // { // // 减少 CPU 占用 // usleep(1000); } } return NULL;
}如果其他的也都开起来,这样就不会出现726ms没有数据输出,726ms后出现连续性粘包持续了300ms;if (pthread_create(&sbus_read_from_dev1_thread_id, NULL, SBUS_READ_FROM_DEV1_thread, (void *)&AR8030_SBUS_SCKT_FD))
{
PrintErr(“Fail to create SBUS_READ_FROM_DEV1_thread!\n”);
return -1;
}
if (pthread_create(&sbus_udprcv_thread_id, NULL, (void *)sBus_UDPRcv_thread, NULL)) { PrintErr(“Fail to create sBus_UDPRcv_thread!\n”); return -1; } if (pthread_create(&sbus_dev1_to_ldsbus_thread_id, NULL, SBUS_DEV1_TO_FDSBUS_thread, &fd_sbus)) { PrintErr(“Fail to create SBUS_DEV_TO_FDSBUS_thread!\n”); return -1; } // if (pthread_create(&sbus_decoe_from_dev_thread_id, NULL, (void *)SBUS_DECODE_FROM_DEV_thread, &fd_sbus)) // { // PrintErr(“Fail to create SBUS_DECODE_FROM_DEV_thread!\n”); // return -1; // } // if (pthread_create(&sbus_decoe_from_udp_thread_id, NULL, (void *)SBUS_DECODE_FROM_UDP_thread, &fd_sbus)) // { // PrintErr(“Fail to create SBUS_DECODE_FROM_UDP_thread!\n”); // return -1; // } // if (pthread_create(&sbus_send_thread_id, NULL, (void *)SBUS_SEND_thread, &fd_sbus)) // { // PrintErr(“Fail to create SBUS_SEND_thread!\n”); // return -1; // } //Telem1 printf(“AR8030_UART_SCKT_FD is %d\n\r”,AR8030_UART_SCKT_FD.bb_dev_skt_fd); if (pthread_create(&uart_recvfrom_dev_thread_id, NULL, (void *)UART_Recvfrom_DEV_thread, &AR8030_UART_SCKT_FD)) { PrintErr(“Fail to create UART_Recvfrom_DEV_thread!\n”); return -1; } if (pthread_create(&telem1_uartsend_thread_id, NULL, (void *)Telem1_UARTsend_thread, &fd_telem1)) { PrintErr(“Fail to create Telem1_UARTsend_thread!\n”); return -1; } if (pthread_create(&telem1_uartrcv_thread_id, NULL, (void *)Telem1_UartRcv_thread, &fd_telem1)) { PrintErr(“Fail to create Telem1_UartRcv_thread!\n\r”); } if (pthread_create(&uart_telem1_to_devfiber_thread_id, NULL, (void *)UART_TELEM1_TO_DEV_fiber_thread, &AR8030_UART_SCKT_FD)) { PrintErr(“Fail to create Telem1_UartRcv_thread!\n\r”); } //telem2 Fixed UDP if (pthread_create(&telem2_uartsend_thread_id, NULL, (void *)Telem2_UARTsend_thread, &fd_telem2)) { PrintErr(“Fail to create Telem2_UARTsend_thread!\n\r”); } if (pthread_create(&telem2_uartrcv_thread_id, NULL, (void *)Telem2_UartRcv_thread, &fd_telem2)) { PrintErr(“Fail to create Telem2_UartRcv_thread!\n\r”); } if (pthread_create(&telem2_udpsend_thread_id, NULL, (void *)Telem2_UDPsend_thread, NULL)) { PrintErr(“Fail to create Telem2_UDPsend_thread!\n\r”); },一定要用这么多线程
最新发布