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);
}
