public void ProcessPacket(SerialPacket packet, ShiftRegisterDriver.Session shiftRegisterSession)
{
this.user = packet.Data.Substring(2, packet.Data.Length - 2);
switch (packet.Data[0])
{
case StatusCode.Away:
this.status = "away";
// first and third bit enabled
shiftRegisterSession.Write(5);
break;
case StatusCode.Busy:
this.status = "busy";
// first and fourth bit enabled
shiftRegisterSession.Write(9);
break;
case StatusCode.Free:
this.status = "free";
// first and second bit enabled
shiftRegisterSession.Write(3);
break;
}
}
private static bool Predicate(SerialPacket arg)
{
if (arg == null)
{
return(false);
}
Console.WriteLine($"Received package: {arg.Payload}");
return(true);
}
internal override byte[] GenerateCommand()
{
List <byte> command = new List <byte>();
command.Add(CommandCode);
command.AddRange(DataConverter.BytesFromShort((short)thousandths_per_second));
command.AddRange(DataConverter.BytesFromFloat(toLocation.X));
command.AddRange(DataConverter.BytesFromFloat(toLocation.Y));
command.AddRange(DataConverter.BytesFromFloat(toLocation.Z));
return(SerialPacket.Packetize(command.ToArray(), 0x21));
}
private static void TestSerialData()
{
Task.Run(() => CheckAlert());
SerialPacket packet = SerialManager.Instnace.Receive();
foreach (var channel in Instance.Channels)
{
if (random.NextDouble() <= 0.3)
{
channel.Value = random.NextDouble() * channel.Max;
}
}
}
private void ProcessPacket(SerialPacket packet, ShiftRegisterDriver shiftRegisterDriver)
{
switch (packet.DataType)
{
case PacketDataType.Lync:
using (var shiftRegisterSession = shiftRegisterDriver.AcquireSessionLock())
{
this.lyncCache.ProcessPacket(packet, shiftRegisterSession);
}
this.lyncCache.QueueDisplayStatus(this.lcdScreen);
break;
default:
var page = this.lcdScreen.CreatePage();
page.Write(0, packet.Data);
this.lcdScreen.PushPage(page);
break;
}
}
/// <summary>
/// sends command to UM6 device
/// </summary>
/// <param name="command"></param>
public void SendCommand(string command)
{
byte address = 0;
switch (command)
{
case "ZeroRateGyros":
address = SerialPacket.UM6_ZERO_GYROS; // see UM2B.XML - UM6_ZERO_GYROS - "Zero Rate Gyros"
// expect packet 11 in response () - two data words, "Bias of x-axis rate gyro" and "Bias of y-axis rate gyro"
break;
case "SetAccelRefVector":
address = SerialPacket.UM6_SET_ACCEL_REF; // see UM2B.XML - UM6_SET_ACCEL_REF - "Set Accelerometer Reference Vector"
// no response expected.
break;
case "SetMagnRefVector":
address = SerialPacket.UM6_SET_MAG_REF; // see UM2B.XML - UM6_SET_MAG_REF - "Set Magnetometer Reference Vector"
// no response expected. The Euler values and quaternion will slowly rotate to have current robot position show as
// Magnetic North (0 degrees). It will take ~10 sec.
break;
}
if (address != 0)
{
// Commands are initiated by executing a read command for the relevant command address using the UART.
SerialPacket NewPacket = new SerialPacket(address);
// If code reaches this point, then there enough bytes in the RX buffer to form a complete packet.
NewPacket.PacketDescriptor = 0; // data length 0, not a batch, doesn't have data
byte[] bytes = NewPacket.ToByteArray(); // will also compute checksum
_serialPort.Write(bytes, 0, bytes.Length);
}
}
protected override void CopyData(SerialPacket oSerialPacket)
{
mdelegate_CopyData(oSerialPacket);
}
/// <summary>
/// Sensor Protocol:
/// S,GX,GY,GZ,AccX,AccY,AccZ,Bat,M1,M2,M3,M4,E
/// Settings Protocol:
/// C,.......................................,E
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void DataReceived(object sender, SerialDataReceivedEventArgs e)
{
string RxString;
RxString = (mSerialPort.ReadExisting());
byte[] array = new byte[8000];
array = Encoding.Unicode.GetBytes(RxString);
for (int i = 0; i < array.Length; ++i) // j; ++i)
{
// Sensors Start Tag .... Data of Gyros & Acc readinsg while flying is being recived.
if ((!bStartCopy) && (array[i] == 'I'))
{
mRxDataType = ENUM_RxDataType.IMU;
bStartCopy = true;
Command = 0;
Idx = 0;
continue;
}
if ((!bStartCopy) && (array[i] == 'S'))
{
mRxDataType = ENUM_RxDataType.Sensors;
bStartCopy = true;
Command = 0;
Idx = 0;
continue;
}
// Configuration Start Tag .... Data of Config structure is being received
if ((!bStartCopy) && (array[i] == 'C'))
{
mRxDataType = ENUM_RxDataType.Settings;
bStartCopy = true;
CommandLength = 999;
Idx = 0;
continue;
}
// End Tag
if ((!bStartCopy) && (array[i] == 'E'))
{
if (ExpectEOF == true) // message correct then copy
{
SerialPacket SP = new SerialPacket();
SP.DataType = mRxDataType;
SP.Array = cArray;
SP.Command = Command;
SP.CommandLength = CommandLength - 2;
CopyData(SP);
}
mRxDataType = ENUM_RxDataType.Undefined;
ExpectEOF = false;
continue;
}
// Incoming Data Copying
if (bStartCopy)
{
switch (mRxDataType)
{
case ENUM_RxDataType.IMU:
if (Idx == 32)
{
Idx = 0;
bStartCopy = false;
ExpectEOF = true;
i -= 1; // stepback to check for 'E'
continue;
}
cArray[Idx] = array[i];
break;
case ENUM_RxDataType.Sensors:
if (Idx == 32)
{
Idx = 0;
bStartCopy = false;
ExpectEOF = true;
i -= 1; // stepback to check for 'E'
continue;
}
cArray[Idx] = array[i];
break;
case ENUM_RxDataType.Settings:
if (Idx == 1)
{
Command = (ENUM_SerialCommands)cArray[Idx-1];
}
if (Idx == 2)
{
CommandLength = cArray[Idx - 1] + cArray[Idx] * 0xff +2 ;
// verify data is OK
if (array[i] == CONST_FIRMWARE_SIGNATURE)
{ // signature is OK
}
}
if (Idx == CommandLength)
{
Idx = 0;
bStartCopy = false;
ExpectEOF = true;
// i -= 1; // stepback to check for 'E'
continue;
}
cArray[Idx] = array[i];
break;
}
Idx += 1;
}
ExpectEOF = false;
}
// Idx += 1; // FIX bug when DataMisalignedException comes CONST_FIRMWARE_SIGNATURE two chunks cArray was shifted by one.
}
public void CopyData(SerialPacket oSerialPacket)
{
int Offset;
switch (oSerialPacket.DataType)
{
case ENUM_RxDataType.Sensors:
case ENUM_RxDataType.IMU:
Offset = 0;
SensorManager.Gyro_X.AddValue((Int16)(BitConverter.ToSingle(oSerialPacket.Array, 0 + Offset)));
SensorManager.Gyro_Y.AddValue((Int16)BitConverter.ToSingle(oSerialPacket.Array, 4 + Offset));
SensorManager.Gyro_Z.AddValue((Int16)BitConverter.ToSingle(oSerialPacket.Array, 8 + Offset));
SensorManager.Acc_X.AddValue((Int16)BitConverter.ToSingle(oSerialPacket.Array, 12 + Offset));
SensorManager.Acc_Y.AddValue((Int16)BitConverter.ToSingle(oSerialPacket.Array, 16 + Offset));
short Z = (Int16)(BitConverter.ToSingle(oSerialPacket.Array, 20 + Offset) - 100);
SensorManager.Acc_Z.AddValue(Z);
SensorManager.Motors[0].AddValue(BitConverter.ToInt16(oSerialPacket.Array, 24 + Offset));
SensorManager.Motors[1].AddValue(BitConverter.ToInt16(oSerialPacket.Array, 26 + Offset));
SensorManager.Motors[2].AddValue(BitConverter.ToInt16(oSerialPacket.Array, 28 + Offset));
SensorManager.Motors[3].AddValue(BitConverter.ToInt16(oSerialPacket.Array, 30 + Offset));
// Log Data into CSV file
mCSVLogFileWriter.LogData();
// UPdate Graph based on Timer Rate not actual received data rate.
if (bRead == true)
{
bRead = false;
chartPlotterGyro.Dispatcher.BeginInvoke(new Action(delegate()
{
SensorManager.Gyro_X.AddGraphValue();
SensorManager.Gyro_Y.AddGraphValue();
SensorManager.Gyro_Z.AddGraphValue();
}));
chartPlotterAcc.Dispatcher.BeginInvoke(new Action(delegate()
{
SensorManager.Acc_X.AddGraphValue();
SensorManager.Acc_Y.AddGraphValue();
SensorManager.Acc_Z.AddGraphValue();
}));
}
break;
case ENUM_RxDataType.Settings:
Offset = 3;
QuadConfigurationManager.QuadConfigStructure.GyroParams[0].SetParameters(oSerialPacket.Array, 0 + Offset);
QuadConfigurationManager.QuadConfigStructure.GyroParams[1].SetParameters(oSerialPacket.Array, 14 + Offset);
QuadConfigurationManager.QuadConfigStructure.GyroParams[2].SetParameters(oSerialPacket.Array, 28 + Offset);
QuadConfigurationManager.QuadConfigStructure.AccParams[0].SetParameters(oSerialPacket.Array, 42 + Offset);
QuadConfigurationManager.QuadConfigStructure.AccParams[1].SetParameters(oSerialPacket.Array, 56 + Offset);
QuadConfigurationManager.QuadConfigStructure.AccParams[2].SetParameters(oSerialPacket.Array, 70 + Offset);
QuadConfigurationManager.QuadConfigStructure.SonarParams[0].SetParameters(oSerialPacket.Array, 84 + Offset);
// QuadConfigurationManager.QuadConfigStructure.VoltageAlarm = vArray[84];
mFlagUpdateSettings = true;
break;
}
}
private int RXBufPtr; // Points to the index in the serial buffer where the next item should be placed
/// <summary>
/// Serial Port data event handler
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
if (e.EventType == SerialData.Chars)
{
int BytesReturned;
bool found_packet;
int packet_start_index;
int packet_index;
int bytes_to_read = 0;
// See UM6 datasheet for more info. Source code in C++ for working with UM6 is at http://sourceforge.net/projects/chrinterface/ (get SVN tarball there)
// The code below is direct translation from C++
// See C:\Projects\Robotics\src\CHR Interface\CHR Interface\SerialConnector.cpp
try
{
bytes_to_read = _serialPort.BytesToRead;
if (bytes_to_read + RXBufPtr >= RX_BUFFER_SIZE)
{
bytes_to_read = RX_BUFFER_SIZE - 1 - RXBufPtr;
}
BytesReturned = _serialPort.Read(RXBuffer, RXBufPtr, bytes_to_read);
RXBufPtr += BytesReturned;
// 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 >= 7)
{
// Search for the packet start sequence
found_packet = false;
packet_start_index = 0;
int skippedBytes = 0;
for (packet_index = 0; packet_index < (RXBufPtr - 2); packet_index++)
{
if (RXBuffer[packet_index] == 's' && RXBuffer[packet_index + 1] == 'n' && RXBuffer[packet_index + 2] == 'p')
{
found_packet = true;
packet_start_index = packet_index;
break;
}
skippedBytes++;
}
// for debugging, see if we are skipping any bytes between the packets:
//if (skippedBytes > 0)
//{
// Console.WriteLine("skipped " + skippedBytes);
//}
// If packet start sequence was not found, then remove all but the last three bytes from the buffer. This prevents
// bad data from filling the buffer up.
if (!found_packet)
{
RXBuffer[0] = RXBuffer[RXBufPtr - 3];
RXBuffer[1] = RXBuffer[RXBufPtr - 2];
RXBuffer[2] = RXBuffer[RXBufPtr - 1];
RXBufPtr = 3;
}
// If a packet start sequence was found, extract the packet descriptor byte.
// Make sure that there are enough bytes in the buffer to consitute a full packet
int indexed_buffer_length = RXBufPtr - packet_start_index;
if (found_packet && (indexed_buffer_length >= 7))
{
byte packet_descriptor = RXBuffer[packet_start_index + 3];
byte address = RXBuffer[packet_start_index + 4];
// Check the R/W bit in the packet descriptor. If it is set, then this packet does not contain data
// (the packet is either reporting that the last write operation was succesfull, or it is reporting that
// a command finished).
// If the R/W bit is cleared and the batch bit B is cleared, then the packet is 11 bytes long. Make sure
// that the buffer contains enough data to hold a complete packet.
bool HasData;
bool IsBatch;
int BatchLength;
int packet_length = 0;
if (((packet_descriptor & 0x80) != 0)) // Has Data?
{
HasData = true;
}
else
{
HasData = false;
}
if (((packet_descriptor & 0x40) != 0)) // Is Batch? (always is, two registers packed together, for example 100 and 101 for quaternions).
{
IsBatch = true;
}
else
{
IsBatch = false;
}
if (HasData && !IsBatch)
{
packet_length = 11;
}
else if (HasData && IsBatch)
{
// from the Datasheet p.24:
// Do some bit-level manipulation to determine if the packet contains data and if it is a batch
// We have to do this because the individual bits in the PT byte specify the contents of the packet.
// uint8_t packet_has_data = (PT >> 7) & 0x01; // Check bit 7 (HAS_DATA)
// uint8_t packet_is_batch = (PT >> 6) & 0x01; // Check bit 6 (IS_BATCH)
// uint8_t batch_length = (PT >> 2) & 0x0F; // Extract the batch length (bits 2 through 5)
// If this is a batch operation, then the packet length is: length = 5 + 4*L + 2, where L is the length of the batch.
// Make sure that the buffer contains enough data to parse this packet.
BatchLength = (packet_descriptor >> 2) & 0x0F;
packet_length = 5 + 4 * BatchLength + 2;
}
else if (!HasData)
{
packet_length = 7;
}
if (indexed_buffer_length < packet_length)
{
return;
}
SerialPacket NewPacket = new SerialPacket();
// If code reaches this point, then there enough bytes in the RX buffer to form a complete packet.
NewPacket.Address = address;
NewPacket.PacketDescriptor = packet_descriptor;
// Copy data bytes into packet data array:
int data_start_index = packet_start_index + 5;
for (int i = 0; i < NewPacket.DataLength; i++)
{
NewPacket.SetDataByte(i, RXBuffer[data_start_index + i]);
}
// Now extract the expected checksum from the packet:
UInt16 Checksum = (UInt16)(((UInt16)RXBuffer[packet_start_index + packet_length - 2] << 8) | ((UInt16)RXBuffer[packet_start_index + packet_length - 1]));
// Compute the checksum on our side and compare with the one given in the packet. If different, ignore this packet.
NewPacket.ComputeChecksum();
if (Checksum == NewPacket.Checksum)
{
OnSerialPacketReceived(NewPacket);
}
else
{
string message = string.Format("Received packet with bad checksum {0} (expected {1}). Packet discarded.", Checksum, NewPacket.Checksum);
// debug:
Console.WriteLine("Error: " + message);
_chrDataPort.Post(new InvalidDataException(Resources.FailedChecksumValidation + " " + message));
}
// At this point, the newest packet has been parsed and copied into the RXPacketBuffer array.
// Copy all received bytes that weren't part of this packet into the beginning of the
// buffer. Then, reset RXbufPtr.
for (int index = 0; index < (RXBufPtr - (packet_start_index + packet_length)); index++)
{
RXBuffer[index] = RXBuffer[(packet_start_index + packet_length) + index];
}
RXBufPtr -= (packet_start_index + packet_length);
}
else
{
return;
}
} // end while RXBufPtr >= 7
}
catch (ArgumentException ex)
{
if (ex.Message == "Offset and length were out of bounds for the array or count is greater than the number of elements from index to the end of the source collection.")
{
Exception invalidBaudRate = new ArgumentException(Resources.InvalidBaudRate);
_chrDataPort.Post(invalidBaudRate);
}
else
{
Exception ex2 = new ArgumentException(Resources.ErrorReadingFromSerialPort, ex);
_chrDataPort.Post(ex2);
}
}
catch (TimeoutException ex)
{
_chrDataPort.Post(ex);
// Ignore timeouts for now.
}
catch (IOException ex)
{
string errorInfo = Resources.ErrorReadingFromSerialPort;
if (bytes_to_read > 0)
{
byte[] b = new byte[bytes_to_read];
int ii = _serialPort.Read(b, 0, bytes_to_read);
errorInfo += "\r\nEnd of buffer: " + ii + " bytes wasted";
}
Exception ex2 = new IOException(errorInfo, ex);
_chrDataPort.Post(ex2);
}
catch (Exception ex)
{
_chrDataPort.Post(ex);
}
}
}
public void OnSerialPacketReceived(SerialPacket packet)
{
//Console.WriteLine(string.Format("packet: {0} {1} {2}", packet.DataLength, packet.IsBatch ? " batch" : "", packet.Address));
// see ...\ChrInterface\UM1B.XML for register addresses
switch (packet.Address)
{
case 92:
/*
* expecting batch of two registers:
* <Register address="92">
* <Name>UM6_GYRO_PROC_XY</Name>
* <Description>X and Y-axis rate gyro output after alignment, bias, and scale compensation have been applied. Stored as two 16-bit signed integers.</Description>
* </Register>
*
* <Register address="93">
* <Name>UM6_GYRO_PROC_Z</Name>
* <Description>Z-axis rate gyro output after alignment, bias, and scale compensation have been applied. Stored as 16-bit signed integer.</Description>
* </Register>
*/
{
// convert 8 bytes into 4 signed shorts:
short[] rates = new short[4];
rates[0] = packet.Address;
for (int i = 0; i < 6; i += 2)
{
rates[i / 2 + 1] = (short)((packet.GetDataByte(i) << 8) + packet.GetDataByte(i + 1));
}
if (doTracePackets)
{
Console.WriteLine(string.Format("UM6_GYRO_PROC: {0} {1} {2}", rates[1], rates[2], rates[3]));
}
_chrDataPort.Post(rates);
}
break;
case 94:
/*
* expecting batch of two registers:
* <Register address="94">
* <Name>UM6_ACCEL_PROC_XY</Name>
* <Description>X and Y-axis accelerometer output after alignment, bias, and scale compensation have been applied. Stored as two 16-bit signed integers.</Description>
* </Register>
*
* <Register address="95">
* <Name>UM6_ACCEL_PROC_Z</Name>
* <Description>Z-axis accelerometer output after alignment, bias, and scale compensation have been applied. Stored as 16-bit signed integer.</Description>
* </Register>
*/
{
// convert 8 bytes into 4 signed shorts:
short[] accels = new short[4];
accels[0] = packet.Address;
for (int i = 0; i < 6; i += 2)
{
accels[i / 2 + 1] = (short)((packet.GetDataByte(i) << 8) + packet.GetDataByte(i + 1));
}
if (doTracePackets)
{
Console.WriteLine(string.Format("UM6_ACCEL_PROC: {0} {1} {2}", accels[1], accels[2], accels[3]));
}
_chrDataPort.Post(accels);
}
break;
case 96:
/*
* expecting batch of two registers:
* <Register address="96">
* <Name>UM6_MAG_PROC_XY</Name>
* <Description>X and Y-axis magnetometer output after soft and hard iron calibration. Stored as 16-bit signed integers.</Description>
* </Register>
*
* <Register address="97">
* <Name>UM6_MAG_PROC_Z</Name>
* <Description>Z-axis magnetometer output after soft and hard iron calibration. Stored as 16-bit signed integer.</Description>
* </Register>
*/
{
// convert 8 bytes into 4 signed shorts:
short[] mags = new short[4];
mags[0] = packet.Address;
for (int i = 0; i < 6; i += 2)
{
mags[i / 2 + 1] = (short)((packet.GetDataByte(i) << 8) + packet.GetDataByte(i + 1));
}
if (doTracePackets)
{
Console.WriteLine(string.Format("UM6_MAG_PROC: {0} {1} {2}", mags[1], mags[2], mags[3]));
}
_chrDataPort.Post(mags);
}
break;
case 98:
/*
* expecting batch of two registers:
* <Register address="98">
* <Name>UM6_EULER_PHI_THETA</Name>
* <Description>Roll and pitch angles. Stored as 16-bit signed integers.</Description>
* </Register>
*
* <Register address="99">
* <Name>UM6_EULER_PSI</Name>
* <Description>Yaw angle in degrees. Stored as 16-bit signed integer.</Description>
* </Register>
*/
{
// convert 8 bytes into 4 signed shorts:
short[] eulers = new short[4];
eulers[0] = packet.Address;
for (int i = 0; i < 6; i += 2)
{
eulers[i / 2 + 1] = (short)((packet.GetDataByte(i) << 8) + packet.GetDataByte(i + 1));
}
if (doTracePackets)
{
Console.WriteLine(string.Format("UM6_EULER: PHI={0} THETA={1} PSI={2}", eulers[1], eulers[2], eulers[3]));
}
_chrDataPort.Post(eulers);
}
break;
case 100:
/*
* expecting batch of two registers:
* <Register address="100">
* <Name>UM6_QUAT_AB</Name>
* <Description>First two elements of attitude quaternion. Stored as two 16-bit signed integers.</Description>
* </Register>
*
* <Register address="101">
* <Name>UM6_QUAT_CD</Name>
* <Description>Third and fourth elements of attitude quaternion. Stored as two 16-bit signed integers.</Description>
* </Register>
*/
{
// convert 8 bytes into 4 signed shorts:
short[] quaternion = new short[5];
quaternion[0] = packet.Address;
for (int i = 0; i < 8; i += 2)
{
quaternion[i / 2 + 1] = (short)((packet.GetDataByte(i) << 8) + packet.GetDataByte(i + 1));
}
if (doTracePackets)
{
Console.WriteLine(string.Format("UM6_QUAT: {0} {1} {2} {3}", quaternion[1], quaternion[2], quaternion[3], quaternion[4]));
}
_chrDataPort.Post(quaternion);
}
break;
default:
//if (doTracePackets)
Console.WriteLine(string.Format("Unexpected packet: Address={0} descr={1:x4} HasData={2} IsBatch={3} BatchLength={4} DataLength={5} CommandFailed={6}",
packet.Address, packet.PacketDescriptor, packet.HasData, packet.IsBatch, packet.BatchLength, packet.DataLength, packet.CommandFailed));
break;
}
}
protected virtual void CopyData(SerialPacket oSerialPacket)
{
throw new NotImplementedException();
}
internal override byte[] GenerateCommand()
{
return(SerialPacket.Packetize(new byte[] { CommandCode }, 0x21));
}
