/// <summary>
/// Sends a packet to the arduino.
/// </summary>
/// <param name="cmd">The command number.</param>
/// <param name="buffer">The bytearray to be sent.</param>
public void Send(int cmd, ArduinoDataType dataType, byte[] buffer)
{
// build serial packet
int checksum = Crc16.ComputeChecksum(buffer);
byte[] serialPacket = new byte[buffer.Length + headerSize];
serialPacket[0] = (byte)(buffer.Length & 0xff);
serialPacket[1] = (byte)((buffer.Length >> 8) & 0xff);
serialPacket[2] = (byte)(checksum & 0xff);
serialPacket[3] = (byte)((checksum >> 8) & 0xff);
serialPacket[4] = (byte)cmd;
serialPacket[5] = (byte)dataType;
Array.Copy(buffer, 0, serialPacket, headerSize, buffer.Length);
// encode packet
byte[] encodedBuffer = new byte[COBS.GetEncodedBufferSize(serialPacket.Length)];
int encodedBufferSize = COBS.Encode(ref serialPacket, serialPacket.Length, ref encodedBuffer);
Array.Resize(ref encodedBuffer, encodedBufferSize + 1);
encodedBuffer[encodedBufferSize] = 0;
// add msg to outgoing queue
lock (sendLockObject)
{
sendPackets.Enqueue(encodedBuffer);
}
}
private void SendDisplayString(string displayString)
{
if (displayString == null || String.Equals(displayString, _lastDisplayStringSent, StringComparison.Ordinal))
{
return;
}
lock (_serialPortLock)
{
try
{
if (_serialPort == null || !_serialPort.IsOpen || displayString == null)
{
return;
}
var cobsEncodedPacket = COBS.Encode(Encoding.ASCII.GetBytes(displayString));
_serialPort.Write(cobsEncodedPacket, offset: 0, count: cobsEncodedPacket.Length);
_serialPort.Write(new byte[] { 0 }, 0, 1);
_serialPort.BaseStream.Flush();
_lastDisplayStringSent = displayString;
_bytesSentSignal.State = cobsEncodedPacket.Length;
}
catch (Exception e)
{
_log.Error(e.Message, e);
}
}
}
/*
* // Arduino serial read asynch callback (doesn't clog up main loop)
* public IEnumerator AsynchronousReadFromArduino(Action<string> callback, Action fail = null, float timeout = float.PositiveInfinity){
* DateTime initialTime = DateTime.Now;
* DateTime nowTime;
* TimeSpan diff = default(TimeSpan);
* string dataString = null;
* do{
* try{
* dataString = arduinoSerialPort.ReadLine();
* }catch (TimeoutException){
* dataString = null;
* }
*
* if (dataString != null){
* callback(dataString);
* yield return null;
* }else{
* yield return new WaitForSeconds(0.05f);
* }
* nowTime = DateTime.Now;
* diff = nowTime - initialTime;
*
* }
* while (diff.Milliseconds < timeout);
* if (fail != null) {
* fail ();
* }
* yield return null;
* }
*/
void sendDriveCommand()
{
if (this.enabled)
{
//print("Sending drive command");
// Unity is in meters, so multiply by 10,000 to get 1mm as base unit
float distErrorf = distanceError_m * 1000.0f;
Int16 distError = (Int16)Math.Round(distErrorf);
if (distErrorf > COMMUNICATION_INT16_MAX)
{
distError = (Int16)(COMMUNICATION_INT16_MAX);
}
else if (distError < -COMMUNICATION_INT16_MAX)
{
distError = (Int16)(-COMMUNICATION_INT16_MAX);
}
// Convert angle and ensure it is in [-180, 180] degrees
float rotationErrorRadians = angleError_deg * Mathf.Deg2Rad;
while (rotationErrorRadians <= -Math.PI) // If we're less than -180 degrees, add a rotation so we're in -180<r<180
{
rotationErrorRadians += twoPi;
}
while (rotationErrorRadians >= Math.PI) // if we're more than +180 degrees, subtract a rotation so we're in -180<r<180
{
rotationErrorRadians -= twoPi;
}
Int16 rotationErrorInt = (Int16)(Math.Round((rotationErrorRadians * COMMUNICATION_INT16_MAX) / Math.PI));
// Good for debugging
// print ("D<" + distError + "mm\t" + ((((float)rotationErrorInt) * 180.0f) / 32767.0f).ToString () + "deg>");
// Byte stuffing, yay!
int dataStartIndex = 0;
driveMessageData [dataStartIndex + 0] = (byte)(distError >> 8);
driveMessageData [dataStartIndex + 1] = (byte)(distError);
driveMessageData [dataStartIndex + 2] = (byte)(rotationErrorInt >> 8);
driveMessageData [dataStartIndex + 3] = (byte)(rotationErrorInt);
int encodededLength = COBS.cobs_encode(ref driveMessageData, DRIVE_MESSAGE_DATA_BYTES, ref COBSEncodedMessage);
// print("Encoded message (" + encodededLength + ")" + BitConverter.ToString(COBSEncodedMessage));
byte[] finalMessage = new byte[encodededLength + 1];
Buffer.BlockCopy(COBSEncodedMessage, 0, finalMessage, 1, encodededLength);
finalMessage [0] = 0;
//print("Initial message (6)" + BitConverter.ToString(driveMessageData) + "\nFinal message: " + BitConverter.ToString(finalMessage));
if (arduinoSerialPort != null && arduinoSerialPort.IsOpen)
{
arduinoSerialPort.Write(finalMessage, 0, encodededLength + 1);
}
//arduinoSerialPort.BaseStream.Flush();
}
}
public void TestEncodingMaxSize()
{
// Create an array of maximum allowed size
var maxSizeArray = new byte[254];
// Attempt to encode the array
var result = COBS.Encode(maxSizeArray);
// Assert the array was created correctly
Assert.True(result.All(c => c == 1));
}
public void TestDecoding()
{
Assert.True(decoded1.SequenceEqual(COBS.Decode(encoded1)));
Assert.True(decoded2.SequenceEqual(COBS.Decode(encoded2)));
Assert.True(decoded3.SequenceEqual(COBS.Decode(encoded3)));
Assert.True(decoded4.SequenceEqual(COBS.Decode(encoded4)));
Assert.True(decoded5.SequenceEqual(COBS.Decode(encoded5)));
Assert.True(decoded6.SequenceEqual(COBS.Decode(encoded6)));
Assert.True(decoded7.SequenceEqual(COBS.Decode(encoded7)));
Assert.True(decoded8.SequenceEqual(COBS.Decode(encoded8)));
Assert.True(decoded9.SequenceEqual(COBS.Decode(encoded9)));
}
private void SendDrawingCommands(string svgPathString)
{
lock (_serialPortLock)
{
try
{
if (_serialPort == null || !_serialPort.IsOpen || svgPathString == null)
{
return;
}
var drawPoints = new SVGPathToVectorScopePointsListConverter(bezierCurveInterpolationSteps: BEZIER_CURVE_INTERPOLATION_STEPS)
.ConvertToDrawPoints(svgPathString)
.ApplyCentering(_config.Centering)
.ApplyInversion(VIEWBOX_WIDTH, VIEWBOX_HEIGHT, invertX: false, invertY: true)
.ApplyRotation(VIEWBOX_WIDTH / 2.0, VIEWBOX_HEIGHT / 2.0, _config.RotationDegrees)
.ApplyScaling(_config.Scaling.ScaleX, _config.Scaling.ScaleY, VIEWBOX_WIDTH, VIEWBOX_HEIGHT)
.ApplyCalibration(_config.XAxisCalibrationData, _config.YAxisCalibrationData)
.ApplyClipping(VIEWBOX_WIDTH, VIEWBOX_HEIGHT)
;
var drawPointsAsBytes = drawPoints.Select(x => (byte[])x).SelectMany(x => x).ToArray();
var cobsEncodedPacket = COBS.Encode(drawPointsAsBytes);
_serialPort.Write(cobsEncodedPacket, offset: 0, count: cobsEncodedPacket.Length);
_serialPort.Write(new byte[] { 0 }, 0, 1);
_serialPort.BaseStream.Flush();
_numDrawPointsSentSignal.State = drawPoints.Count();
_bytesSent.State = cobsEncodedPacket.Length;
}
catch (Exception e)
{
_log.Error(e.Message, e);
}
}
}
public void Decoding8()
{
byte[] expected8 = COBS.Decode(encoded8);
Assert.IsTrue(expected8.SequenceEqual(decoded8));
}
public void Decoding2()
{
byte[] expected2 = COBS.Decode(encoded2);
Assert.IsTrue(expected2.SequenceEqual(decoded2));
}
public void TestEncodingEmpty()
{
Assert.True(new List <byte>().SequenceEqual(COBS.Encode(new List <byte>())));
}
public void TestEncodingNull()
{
Assert.Null(COBS.Encode(null));
}
public void Decoding10()
{
byte[] expected10 = COBS.Decode(encoded10);
Assert.IsTrue(expected10.SequenceEqual(decoded10));
}
public void Decoding3()
{
byte[] expected3 = COBS.Decode(encoded3);
Assert.IsTrue(expected3.SequenceEqual(decoded3));
}
public void Decoding1()
{
byte[] expected1 = COBS.Decode(encoded1);
Assert.IsTrue(expected1.SequenceEqual(decoded1));
}
public void Decoding4()
{
byte[] expected4 = COBS.Decode(encoded4);
Assert.IsTrue(expected4.SequenceEqual(decoded4));
}
public void Decoding7()
{
byte[] expected7 = COBS.Decode(encoded7);
Assert.IsTrue(expected7.SequenceEqual(decoded7));
}
public void Decoding6()
{
byte[] expected6 = COBS.Decode(encoded6);
Assert.IsTrue(expected6.SequenceEqual(decoded6));
}
public void Decoding5()
{
byte[] expected5 = COBS.Decode(encoded5);
Assert.IsTrue(expected5.SequenceEqual(decoded5));
}
/// <summary>
/// Threaded function. Loops and checks for incoming packets.
/// </summary>
private void ReceiveThreadDoTask()
{
try
{
byte[] receiveBuffer = new byte[bufferSize];
int bytesReceived = 0;
while (running)
{
while (sendPackets.Count > 0)
{
byte[] serialPacket;
lock (sendLockObject)
{
serialPacket = sendPackets.Dequeue();
}
stream.Write(serialPacket, 0, serialPacket.Length);
}
if (stream.BytesToRead > 0)
{
int x = stream.ReadByte();
if (x == -1)
{
// Debug.LogError("End of Stream!");
// running = false;
continue;
}
if (x == 0)
{
// decode
byte[] decodedBuffer = new byte[receiveBuffer.Length];
int decodedBufferSize = COBS.Decode(ref receiveBuffer, bytesReceived, ref decodedBuffer);
Array.Resize(ref decodedBuffer, decodedBufferSize);
// disect packt
int len = decodedBuffer[0] | (decodedBuffer[1] << 8);
int checksum = decodedBuffer[2] | (decodedBuffer[3] << 8);
int cmd = decodedBuffer[4];
ArduinoDataType dataType = (ArduinoDataType)decodedBuffer[5];
byte[] dataPacket = new byte[decodedBuffer.Length - headerSize];
Array.Copy(decodedBuffer, headerSize, dataPacket, 0, decodedBufferSize - headerSize);
// check length
if (dataPacket.Length != len)
{
Debug.Log(String.Format("Invalid Packet Length. Want: {0} - Have: {1}", len, dataPacket.Length));
continue;
}
// checksum
int packetChecksum = Crc16.ComputeChecksum(dataPacket);
if (checksum != packetChecksum)
{
Debug.Log(String.Format("Invalid Checksum. Want: {0} - Have: {1}", checksum, packetChecksum));
continue;
}
bytesReceived = 0;
// store in queue
lock (receiveLockObject)
{
receivedPackets.Enqueue(new ArduinoBuffer(cmd, dataType, dataPacket));
}
}
else
{
if (bytesReceived >= bufferSize)
{
// TODO handle differently!
bytesReceived = 0;
}
else
{
receiveBuffer[bytesReceived++] = (byte)x;
}
}
}
}
}
catch (ThreadAbortException ex)
{
Debug.Log("Aborting UDP Receiver Thread!");
// Debug.LogException(ex);
}
}
public void Decoding9()
{
byte[] expected9 = COBS.Decode(encoded9);
Assert.IsTrue(expected9.SequenceEqual(decoded9));
}
