public void LOG_SETUP_moveSpeed(byte INSTRUCTION_BYTE, C_Value _angle, C_Value _speed) { string prefix = "ODD_MOVE"; switch (INSTRUCTION_BYTE) { case (C_DynAdd.INS_WRITE): prefix = "ORDER_move"; break; case (C_DynAdd.INS_REG_WRITE): prefix = "REGISTER_move"; break; case (C_DynAdd.INS_READ): prefix = "READ_move"; break; } if (_speed.Dec != C_DynVal.SET_MOV_SPEED_NOCONTROL) { LOG(String.Format("{0}:[angle];[speed] =\t[{1}]; [{3}] =\t{2:0.00}°;\t{4:0.00}%, [angle-default]=\t{5:0.00}°", prefix, C_CONV.byteArray2strHex_space(_angle.Hex.Reverse().ToArray()), _angle.Dec, C_CONV.byteArray2strHex_space(_speed.Hex.Reverse().ToArray()), _speed.Dec, _angle.Dec_FromDefault )); } else { LOG(String.Format("{0}: [angle];[speed] = [{1}];[{3}] = {2}°; No speed control", prefix, C_CONV.byteArray2strHex_space(_angle.Hex.Reverse().ToArray()), _angle.Dec, C_CONV.byteArray2strHex_space(_speed.Hex.Reverse().ToArray()) )); } }
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% #endregion Writing //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% #region Reading //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% public static bool START_withPacketStartBytes(List <byte> packetBytes) { int q = 0; foreach (byte by in C_DynAdd.PACKETSTART) { if (packetBytes[q] != by) { LOG_unimportant(string.Format( "This packet does not start with PACKETSTART bytes: [{0}]", C_CONV.byteArray2strHex_space(packetBytes.ToArray()) )); return(false); } } return(true); }
public static void ACTUALIZE_motorRegister(e_rot rot, e_regByteType type, List <byte> pars) { if (pars.Count > 0) { byte addressByte = pars[0]; byte[] parValues = pars.Skip(1).ToArray(); C_Packet.LOG_statusPacket(string.Format( "Status OK - actualizing mot[{0}] register type[{1}]: From address[{2}]=[{3}], these values[{4}]", rot, type, addressByte, MainWindow.Ms[rot].Reg.GET_name(addressByte), C_CONV.byteArray2strHex_space(parValues))); foreach (byte byteValue in parValues) { C_SPI.LOG_unimportant(string.Format( "going to acualize mot[{0}] register on address [{1}]", rot, addressByte )); MainWindow.Ms[rot].ACTUALIZE_register(addressByte, byteValue, type); addressByte++; } } }
public static bool PROCESS_receivedPacket(List <byte> receivedBytes) { // we have received one whole packet /* * -> is Consistent ? * -> pair with best from lastSent queues * -> PROCESS_statusPacket(received, paired) */ try { C_Packet received = new C_Packet(receivedBytes); // constructor checks consistency received.statusReceivedTime = DateTime.Now; return(PAIR_andProcessStatusPacket(received)); } catch (Exception e) { LOG_debug(string.Format( "Will not process received packet because of its consistency error: {0} Exception:{1}", C_CONV.byteArray2strHex_space(receivedBytes), e.Message )); return(false); } }
private static void SPI_DataReceivedHandler(object sender, SerialDataReceivedEventArgs e) { lock (spiGot_locker) { SerialPort sp = (SerialPort)sender; Queue <byte> readBuffer = new Queue <byte>(); byte receivedByte = 0; LOG_unimportant("Start to read packet"); try { // when item is true -> lastSent packet for this motor (index) was already processed // load new lastSent packet to this index from queueSent[index] //int cnt = 0; bool INCOMING_PACKET = false;//= START_withPacketStartBytes(packetBytes); if (receivingPacketBytes.Count <= GET_packetLength(receivingPacketBytes)) { if (PACKETSTART_detector(receivingPacketBytes) == 0) { INCOMING_PACKET = true; } } int packetLength = GET_packetLength(receivingPacketBytes); // read all the bytes to read while (0 != sp.BytesToRead) { receivedByte = (byte)C_SPI.spi.ReadByte(); readBuffer.Enqueue(receivedByte); } while (readBuffer.Count != 0) { receivedByte = readBuffer.Dequeue(); receivingPacketBytes.Add(receivedByte); //cnt = packetBytes.Count; if (receivingPacketBytes.Count >= 2) { // PACKETSTART detector - detect whether the start sequence is not only in the first byte but wherever in the packetBytes array int i_start = PACKETSTART_detector(receivingPacketBytes); if (i_start == 0) { INCOMING_PACKET = true; } else if (i_start > 0) { LOG_debug(string.Format( "Found another PACKETSTART sequence inside recieving packetBytes on index [{0}] in : {1}" + "\nthis is not allowed - skip this packet and use the next PACKETSTART sequence", i_start, C_CONV.byteArray2strHex_space(receivingPacketBytes.ToArray()) )); receivingPacketBytes.RemoveRange(0, receivingPacketBytes.Count - i_start - 1); // -1?? // try to use the now trimmed part of packet as a full packet ? INCOMING_PACKET = true; continue; } else if (i_start == -1) { LOG_debug("Did not found any PACKETSTART sequence in receiving packet : " + C_CONV.byteArray2strHex_space(receivingPacketBytes.ToArray()).ToString()); INCOMING_PACKET = false; continue; } else { LOG_debug("Not allowed state, the PACKETSTART not detected nor detected - INCOMING_PACKET=false"); INCOMING_PACKET = false; } if (INCOMING_PACKET == true) { if (receivingPacketBytes.Count - 1 == C_DynAdd.INDEXOF_ID_IN_STATUSPACKET) { if (receivedByte > C_DynAdd.ID_BROADCAST) { LOG_debug("Not allowed value of id : " + receivedByte.ToString()); receivingPacketBytes.RemoveAt(0); // remove the first byte as it disables the PACKETSTART detection with positioning the id byte as this INCOMING_PACKET = false; continue; // it will be catched by PACKETSTART detector } } if (receivingPacketBytes.Count - 1 == C_DynAdd.INDEXOF_LENGTH_IN_STATUSPACKET) // get the LENGTH_BYTE { packetLength = GET_packetLength(receivingPacketBytes); if (packetLength > C_DynAdd.MAX_BYTES_OF_PACKET) { // too long packet LOG_debug(string.Format( "Packet length byte greater than allowed = {0} > {1}", packetLength, C_DynAdd.MAX_BYTES_OF_PACKET )); receivingPacketBytes.RemoveAt(0); // remove the first byte as it disables the PACKETSTART detection with positioning the id byte as this INCOMING_PACKET = false; continue; } if (packetLength < C_DynAdd.MIN_BYTES_OF_PACKET) { // too short packet LOG_debug(string.Format( "Packet length byte smaller than allowed = {0} < {1}", packetLength, C_DynAdd.MIN_BYTES_OF_PACKET )); receivingPacketBytes.RemoveAt(0); // remove the first byte as it disables the PACKETSTART detection with positioning the id byte as this INCOMING_PACKET = false; continue; } // LENGTH_BYTE = N*[Params] + 1*[LEN] + 1*[INS/ERROR] // packetLength = [LENGTH_BYTE] + 1*[ID] + 1*[CheckSum] + 2*[PacketStart] //packetLength = (int)receivedByte + 4; } packetLength = GET_packetLength(receivingPacketBytes); if (receivingPacketBytes.Count >= packetLength) { // the last added byte to packetBytes was the most likely last byte of this package INCOMING_PACKET = false; LOG_cmd(receivingPacketBytes.ToArray(), e_cmd.received); LOG_unimportant("end of packet"); // chose lastSent by receivedPacketBytes idByte List <byte> statusBytes = new List <byte>(receivingPacketBytes); receivingPacketBytes.Clear(); LOG_debug("Sent packetBytes to process and 'cleared from readBuffer' :" + C_CONV.byteArray2strHex_space(statusBytes.ToArray())); // process it PROCESS_receivedPacket(statusBytes); } if (receivingPacketBytes.Count > packetLength) { // trim the packet LOG_err(String.Format( "Packet too long, there were more bytes read from packet than the length byte proposes: " + "{0} from {1}! LENBYTE={2}! \nIt will by cutted! : {3}", receivingPacketBytes.Count, packetLength, receivingPacketBytes[C_DynAdd.INDEXOF_LENGTH_IN_STATUSPACKET], C_CONV.byteArray2strHex_space(receivingPacketBytes.ToArray()) )); receivingPacketBytes = receivingPacketBytes.GetRange(0, GET_packetLength(receivingPacketBytes)); } } } if (C_State.prog == e_stateProg.closing) { return; } } } catch (Exception ex) { LOG_err("Exception in packet read algorithm :" + GET_exInfo(ex)); //FLUSH_forNextIncomingPackage("exception"); } // try if there is not a full packet in the packetBytes if (receivingPacketBytes.Count == GET_packetLength(receivingPacketBytes)) { if (PACKETSTART_detector(receivingPacketBytes) == 0) { PROCESS_receivedPacket(receivingPacketBytes); receivingPacketBytes.Clear(); } } } }