private void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
int continue_parsing = 1;
int bytes_to_read;
byte packet_type = 1;
//dimensione fissa del pacchetto
Int16 data_size = 85;//full packet struct
Int16 data_size_onlyquat = 24;//only quat packet struct
Int16 data_size_configuration = 240;//oconfiguration packet struct
byte[] data = new byte[MAX_PACKET_SIZE];
byte[] dataconfiguration = new byte[data_size_configuration];
//serialPort.DiscardInBuffer();
bool found_packet;
bool found_packet_congiguration;
bool found_packet_only_quat = false;
int packet_start_configuration_index;
int packet_start_index;
int packet_start_index_only_quat;
int packet_index;
try
{
bytes_to_read = serialPort.BytesToRead;
if (bytes_to_read >= data_size_configuration)
{
if ((RXbufPtr + bytes_to_read) >= RX_BUF_SIZE)
{
RXbufPtr = 0;
}
if (bytes_to_read >= RX_BUF_SIZE)
{
bytes_to_read = RX_BUF_SIZE - 1;
}
// Get serial data
serialPort.Read(RXbuffer, RXbufPtr, bytes_to_read);
// serialPort.DiscardInBuffer();
RXbufPtr += bytes_to_read;
}
}
catch
{
COMFailedEvent();
return;
}
// If there are enough bytes in the buffer to construct a full packet, then check data.
// There are RXbufPtr bytes in the buffer at any given time
while (RXbufPtr >= data_size && (continue_parsing == 1))
{
// Search for the packet start sequence
found_packet = false;
packet_start_index = 0;
found_packet_congiguration = false;
found_packet_only_quat = false;
packet_start_configuration_index = 0;
packet_start_index_only_quat = 0;
for (packet_index = 0; packet_index < (RXbufPtr - 2); packet_index++)
{
//if (RXbuffer[packet_index] == '#' && RXbuffer[packet_index + 110] == '*')
if (RXbuffer[packet_index] == '#' && RXbuffer[packet_index + 1] == 's' && RXbuffer[packet_index + 2] == 'n')
{
if (RXbuffer[packet_index + 23] == '*')//only quat data
{
found_packet_only_quat = true;
packet_start_index_only_quat = packet_index;
break;
}
else
{
found_packet = true;
packet_start_index = packet_index;
break;
}
}
if (RXbuffer[packet_index] == '#' && RXbuffer[packet_index + 1] == 's' && RXbuffer[packet_index + 2] == 'p' && RXbuffer[packet_index + 3] == 'p')
{
found_packet_congiguration = true;
packet_start_configuration_index = packet_index;
break;
}
}
// If start sequence found, try to recover all the data in the packet
if (found_packet && ((RXbufPtr - packet_start_index) >= data_size))
{
// Only process packet if data_size is not too large.
if (data_size <= MAX_PACKET_SIZE)
{
for (int i = 0; i < data_size; i++)
{
data[i] = 0;
}
// If a full packet has been received, then the full packet size should be
// 3 + 1 + 1 + [data_size] + 2
// that is, 3 bytes for the start sequence, 1 byte for type, 1 byte for data length, data_size bytes
// for packet data, and 2 bytes for the checksum.
// If enough data has been received, go ahead and recover the packet. If not, wait until the
// rest of the data arrives
int buffer_length = (RXbufPtr - packet_start_index);
int packet_length = (4 + data_size);
if (buffer_length >= packet_length)
{
// A full packet has been received. Retrieve the data.
for (int i = 0; i < data_size; i++)
{
data[i] = RXbuffer[packet_start_index + i];
RXbuffer[packet_start_index + i]=0;
//Console.WriteLine("data {0}", data[i].ToString());
}
handle_packet(packet_type, (int)data_size, data);
}
else
{
continue_parsing = 0;
}
}
else
{
// data_size was too large - the packet data is invalid. Clear the RX buffer.
RXbufPtr = 0;
continue_parsing = 0;
//AppendStatusText("\r\nBAD PACKET", Color.Red);
}
}
// If start sequence found, try to recover all the data in the packet
if (found_packet_only_quat && ((RXbufPtr - packet_start_index_only_quat) >= data_size_onlyquat))
{
// Only process packet if data_size is not too large.
if (data_size_onlyquat <= MAX_PACKET_SIZE)
{
for (int i = 0; i < data_size_onlyquat; i++)
{
data[i] = 0;
}
// If a full packet has been received, then the full packet size should be
// 3 + 1 + 1 + [data_size] + 2
// that is, 3 bytes for the start sequence, 1 byte for type, 1 byte for data length, data_size bytes
// for packet data, and 2 bytes for the checksum.
// If enough data has been received, go ahead and recover the packet. If not, wait until the
// rest of the data arrives
int buffer_length = (RXbufPtr - packet_start_index_only_quat);
int packet_length = (4 + data_size_onlyquat);
if (buffer_length >= packet_length)
{
// A full packet has been received. Retrieve the data.
for (int i = 0; i < data_size_onlyquat; i++)
{
data[i] = RXbuffer[packet_start_index_only_quat + i];
RXbuffer[packet_start_index_only_quat + i] = 0;
//Console.WriteLine("data {0}", data[i].ToString());
}
handle_packet_ony_quat(packet_type, (int)data_size, data);
}
else
{
continue_parsing = 0;
}
}
else
{
// data_size was too large - the packet data is invalid. Clear the RX buffer.
RXbufPtr = 0;
continue_parsing = 0;
//AppendStatusText("\r\nBAD PACKET", Color.Red);
}
}
//found configuration paket
if (found_packet_congiguration && (((RXbufPtr - packet_start_configuration_index) >= data_size_configuration)))
{
for (int i = 0; i < data_size_configuration; i++)
{
dataconfiguration[i] = 0;
}
// If a full packet has been received, then the full packet size should be
// 3 + 1 + 1 + [data_size] + 2
// that is, 3 bytes for the start sequence, 1 byte for type, 1 byte for data length, data_size bytes
// for packet data, and 2 bytes for the checksum.
// If enough data has been received, go ahead and recover the packet. If not, wait until the
// rest of the data arrives
int buffer_length = (RXbufPtr - packet_start_configuration_index);
int packet_length = (4 + data_size_configuration);
if (buffer_length >= packet_length)
{
// A full packet has been received. Retrieve the data.
for (int i = 0; i < data_size_configuration; i++)
{
dataconfiguration[i] = RXbuffer[packet_start_configuration_index + i];
RXbuffer[packet_start_configuration_index + i] = 0;
//Console.WriteLine("data {0}", data[i].ToString());
}
handle_packet_configuration(packet_type, (int)data_size_configuration, dataconfiguration);
}
else
{
continue_parsing = 0;
}
RXbufPtr = 0;
continue_parsing = 0;
}
else
{
continue_parsing = 0;
}
}
}
