public void pkg_decode(byte msg_cnt) { message_t[] rx_msg = new message_t[64]; for (int i = 0; i < msg_cnt; i++) { // 拆包 comlink_get_msg(ref rx_msg[i], (byte)i); switch (rx_msg[i].msgid) { case 0: break; case 1: //attitude2[i] = byte_to_struct<__attitude_t>(rx_msg[i].payload); break; case 2: break; case 3: break; case 4: break; } } }
public void test_parse() { // 封包 message_t tx_msg = new message_t(); __attitude_t attitude; attitude.pitch = 89.12f; attitude.pitchspeed = 0.12f; attitude.roll = 178.23f; attitude.rollspeed = 1.21f; attitude.time_boot_ms = 12345678; attitude.yaw = 272.78f; attitude.yawspeed = 2.65f; byte pkg_length = (byte)Marshal.SizeOf(typeof(__attitude_t)); byte[] pkg = struct_to_byte <__attitude_t>(attitude); comlink_encode(ref tx_msg, ref pkg[0], 1, 2, pkg_length); serial_trans(ref tx_msg, pkg_length); // 拆包 __attitude_t attitude1 = byte_to_struct <__attitude_t>(tx_msg.payload); // 模拟解析数据 int msg_number = 10; int msg_length = pkg_length + 8; byte[] msg_buffer = new byte[msg_length * msg_number]; for (int i = 0; i < msg_number; i++) { attitude.pitch = 10 + i; byte[] buffer = struct_to_byte <__attitude_t>(attitude); comlink_encode(ref tx_msg, ref buffer[0], 1, 2, pkg_length); byte[] buffer1 = struct_to_byte <message_t>(tx_msg); Array.Copy(buffer1, 0, msg_buffer, i * msg_length, msg_length); } // 消息和数据包缓存 message_t[] rx_msg = new message_t[64]; __attitude_t[] attitude2 = new __attitude_t[64]; // 解析 byte msg_cnt = comlink_parse(ref msg_buffer[0], msg_length * msg_number); for (int i = 0; i < msg_cnt; i++) { // 拆包 comlink_get_msg(ref rx_msg[i], (byte)i); attitude2[i] = byte_to_struct <__attitude_t>(rx_msg[i].payload); } }
public static extern void comlink_get_msg(ref message_t msg, byte number);
public static extern byte comlink_encode(ref message_t msg, ref byte packet, byte compid, byte msg_id, byte length);
public static void serial_trans(ref message_t msg, int pkg_size) { byte[] buffer = struct_to_byte <message_t>(msg); //trans(buffer, pkg_size); }