解析函数应该是没有问题,因为是直接移植的以前用的芯片上的解析函数。为避免问题,上贴串口解析,请大哥们给点意见。[C] 纯文本查看 复制代码 struct smart_telemetry_package *
SMART_Telemetry_Recieve_Datas(uintptr_t telemetry_id) {
int32_t cmd_id = -1;
if (!telemetry_id) {
return NULL;
}
struct smart_telemetry_dev *dev = (struct smart_telemetry_dev *)telemetry_id;
if (!dev->has_remain) {
uint16_t buf_len = NUM_OF_ARRAY(dev->receive_buf);
memset(dev->receive_buf, 0, buf_len);
uint16_t len = 0;
if ((len = comGetChar(COM_TELEMETRY, dev->receive_buf)) > 0) {
// parse data
dev->last_read_len = len;
cmd_id = SMART_Telemetry_Parse_Datas(dev, dev->receive_buf, 0, len);
}
} else {
cmd_id = SMART_Telemetry_Parse_Datas(
dev, dev->receive_buf, dev->check_sum_pos + 1, dev->last_read_len);
if (cmd_id < 0) {
dev->last_read_len = 0;
dev->check_sum_pos = 0;
dev->has_remain = false;
}
}
if (cmd_id > 0) {
return &dev->recieve_package;
}
return NULL;
}
[C] 纯文本查看 复制代码 int32_t SMART_Telemetry_Parse_Datas(struct smart_telemetry_dev *dev,
uint8_t *buffer, uint8_t offset,
uint16_t len) {
bool check_ok = false;
for (uint8_t i = offset; i < len; i++) {
uint8_t data = buffer[i];
if (dev->parse_state == PARSE_STATE_HEAD0) {
if (data == TELEMETRY_PROTOCOL_HEAD0) {
dev->parse_state = PARSE_STATE_HEAD1;
dev->recieve_package.head0 = data;
}
} else if (dev->parse_state == PARSE_STATE_HEAD1) {
if (data == TELEMETRY_PROTOCOL_HEAD1) {
dev->parse_state = PARSE_STATE_HEAD2;
dev->recieve_package.head1 = data;
} else {
dev->parse_state = PARSE_STATE_HEAD0;
}
} else if (dev->parse_state == PARSE_STATE_HEAD2) {
if (data == TELEMETRY_PROTOCOL_HEAD2) {
dev->parse_state = PARSE_STATE_DATA_LEN;
dev->recieve_package.head2 = data;
} else {
dev->parse_state = PARSE_STATE_HEAD0;
}
dev->recieve_package.len_recv_idx = 0;
dev->recieve_package.len = 0;
} else if (dev->parse_state == PARSE_STATE_DATA_LEN) {
dev->recieve_package.len |=
(data << (dev->recieve_package.len_recv_idx * 8));
if (dev->recieve_package.len_recv_idx < 1) {
dev->recieve_package.len_recv_idx++;
} else {
if (dev->recieve_package.len >= NELEMENTS(dev->recieve_package.data)) {
dev->parse_state = PARSE_STATE_HEAD0;
} else {
dev->parse_state = PARSE_STATE_CMD;
}
}
} else if (dev->parse_state == PARSE_STATE_CMD) {
dev->recieve_package.cmd = data;
dev->recieve_package.data_recv_idx = 0;
if (dev->recieve_package.len <= 0) {
dev->parse_state = PARSE_STATE_CHECK;
} else {
dev->parse_state = PARSE_STATE_DATA;
}
} else if (dev->parse_state == PARSE_STATE_DATA) {
if (dev->recieve_package.data_recv_idx <
NELEMENTS(dev->recieve_package.data)) {
dev->recieve_package.data[dev->recieve_package.data_recv_idx++] = data;
if (dev->recieve_package.data_recv_idx >= (dev->recieve_package.len)) {
dev->parse_state = PARSE_STATE_CHECK;
}
} else {
dev->recieve_package.data_recv_idx = 0;
dev->recieve_package.len_recv_idx = 0;
dev->recieve_package.len = 0;
dev->parse_state = PARSE_STATE_HEAD0;
}
} else if (dev->parse_state == PARSE_STATE_CHECK) {
dev->recieve_package.check = data;
dev->parse_state = PARSE_STATE_HEAD0;
check_ok = (SMART_Telemetry_Checksum_Package((uintptr_t)dev) == data);
dev->has_remain = (i != (len - 1));
dev->check_sum_pos = i;
break;
}
}
if (check_ok) {
return dev->recieve_package.cmd;
}
return -1;
} |