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