public static bool IS_statusPacketFollowing(C_Packet lastSent)
{
if (
(lastSent.byteId == C_DynAdd.ID_BROADCAST) // never no matter of returnStatusLevel
||
(lastSent.returnStatusLevel == e_statusReturnLevel.never)
)
{
return(false);
}
else if (
(lastSent.byteInstructionOrError == C_DynAdd.INS_PING)
||
(lastSent.returnStatusLevel == e_statusReturnLevel.allways) // always no matter of returnStatusLevel
)
{
return(true);
}
else
{
// (lastSent.returnStatusLevel == e_returnStatusLevel.onRead)
if (lastSent.byteInstructionOrError == C_DynAdd.INS_READ)
{
return(true);
}
else
{
return(false);
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endregion static functions
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
public bool IS_answerOf(C_Packet lastSent)
{
// checks if this packet is possible status message of lastSent packet
// byteId
// length of bytes wanted
if (lastSent.ByteId == ByteId)
{
if (lastSent.ByteInstructionOrError == C_DynAdd.INS_READ)
{
int numOfWantedParams = lastSent.Par[1];
if (Par.Count == numOfWantedParams)
{
// this is probably the one
return(true);
}
else
{
return(false); // not suitable number of parameters
}
}
else
{
return(false); // last packet wasn't a read packet
}
}
else
{
return(false);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endregion INIT
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#region Sending
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
public void SEND_example(int num)
{
C_Packet.SEND_packet(
new C_Packet(
this, cmdPackets[num].ByteInstructionOrError, cmdPackets[num].Par
));
}
public static bool FIND_bestPairInLastSentQueues(C_Packet received, ref C_Packet paired)
{
// return through ref the best pairedPacket
e_rot rot;
bool foundMotor = C_MotorControl.GET_motorRotFromId(received.ByteId, out rot);
int rotMot = (int)rot;
if (foundMotor == true)
{
lock (queueSent_locker)
{
if (queueSent[rotMot].Count > 0)
{
List <C_Packet> listSent = (queueSent[rotMot]).ToList();
bool foundBestPair = FIND_bestPairInQueue(received, ref paired, ref listSent);
queueSent[rotMot] = new Queue <C_Packet>(listSent);
queueSent_Count[rotMot] = listSent.Count;
C_MotorControl.ACTUALIZE_queueCounts(queueSent);
return(foundBestPair);
}
else
{
LOG_debug(string.Format("lastSent queue of this motor[{0}] was empty!", rot));
return(false);
}
}
}
else
{
LOG_debug("Did not found any motor connected with this StatusPacket id : "
+ received.ByteId.ToString());
}
return(false);
}
public static void PROCESS_instructionPacket(C_Packet lastSent)
{
if (IS_packetChangingValue(lastSent) == true)
{
if (lastSent.byteId != C_DynAdd.ID_BROADCAST)
{
C_MotorControl.ACTUALIZE_motorRegister(
lastSent.rotMotor,
e_regByteType.sentValue, // as we written them just now
lastSent.Par);
LOG_instruPacket(string.Format(
"Written OK (blind) - actualizing motor [{1}] register sentValue: \t{0}",
lastSent.PacketBytes_toString, lastSent.rotMotor));
}
else
{
// parse it and process each part alone
//LOG_instruPacket(string.Format(
// "Written OK (blind) - actualizing motors (broadcast) register sentValue: \t{0}",
// lastSent.PacketBytes_toString));
LOG_instruPacket(string.Format(
"Written OK (blind) - NOT actualizing motors (broadcast) register sentValue: \t{0}",
lastSent.PacketBytes_toString));
}
}
}
public static bool PAIR_andProcessStatusPacket(C_Packet received)
{
//LOG_debug("index = " + index.ToString());
C_Packet pairedLastSent = new C_Packet();
bool foundBestPair = FIND_bestPairInLastSentQueues(received, ref pairedLastSent);
// acquire the unprocessed lastSent package from queue if needed
if (foundBestPair == true)
{
LOG_debug(string.Format(
"Paired status package: {0}\n with this sentPackage: {1}",
received.PacketBytes_toString,
pairedLastSent.PacketBytes_toString
));
LOG_debug("now processing packet");
// process paired lastSent and this statusPacket
try
{
C_Packet.PROCESS_statusPacket(received, pairedLastSent);
}
catch (Exception ex)
{
LOG_err("Exception in processing received packet :" + GET_exInfo(ex));
}
}
return(false);
}
public static void LOG_err(C_Packet packet, string _msg)
{
LOG_packet(string.Format(
"Error packet:\n{0}\n{1}",
GET_packetInfo(packet),
_msg
));
}
public static string GET_packetInfo(C_Packet packet)
{
return(string.Format(
"MotId[{0}] rot[{1}] returnStatusLevel[{2}]\ttype[{3}]\tBytes[{4}]",
packet.ByteId, GET_rotMotorInfo(packet),
packet.returnStatusLevel, GET_motorDataTypeInfo(packet),
packet.PacketBytes_toString
));
}
public C_Packet(C_Packet packet)
{
byteId = packet.byteId;
byteInstructionOrError = packet.ByteInstructionOrError;
byteLength = packet.byteLength;
byteCheckSum = packet.byteCheckSum;
Par = packet.Par;
returnStatusLevel = packet.returnStatusLevel;
motorDataType = packet.motorDataType;
}
private static void QUEUE_PacketToSent(C_Packet instructionPacket)
{
// adds data to sending queue
lock (queueToSent_locker)
{
LOG("enqueue instructionPacket");
queueToSent.Enqueue(new C_Packet(instructionPacket));
LOG("enqueued instructionPacket");
}
}
public bool IS_error()
{
// checks error byte
if (PacketBytes[C_DynAdd.INDEXOF_ERROR_IN_STATUSPACKET] != 0)
{
// it is error - log error and dont process anything else as it won't come
C_Packet.LOG_errorByte(this);
return(true);
}
return(false);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#region LOG
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//public static void LOG_sent(C_Packet packet)
//{
// LOG_instruPacket(string.Format(
// "Sent Instruction packet!\n{0}", GET_packetInfo(packet)
// ));
//}
//public static void LOG_echo(C_Packet packet)
//{
// LOG_statusPacket(string.Format(
// "Got Status packet!\n{0}", GET_packetInfo(packet)
// ));
//}
public static void LOG_ex(C_Packet packet, Exception ex)
{
if (packet.IsConsistent == false)
{
LOG_packet(string.Format(
"Inconsistent packet:\n{0}\n{1}",
GET_packetInfo(packet),
GET_exceptionInfo(ex)
));
}
}
public static string GET_rotMotorInfo(C_Packet packet)
{
if (packet.byteId == C_DynAdd.ID_BROADCAST)
{
return("Broadcast");
}
else
{
return(packet.rotMotor.ToString());
}
}
public static string GET_errorByteInfo(C_Packet packet)
{
StringBuilder str = new StringBuilder();
for (int b = 0; b < 7; b++)
{
if (C_CONV.GET_bit(packet.byteInstructionOrError, b) == true)
{
str.AppendLine(string.Format("ID[{0}] error: {1}", packet.byteId, errStr[b]));
}
}
return(str.ToString());
}
public static void ACTUALIZE_motorRegistersFromStatusPacket(C_Packet received, C_Packet pairedLastSent)
{
try
{
if (received.byteId == pairedLastSent.byteId)
{
// we have no error in statusPacket
if (pairedLastSent.byteInstructionOrError == C_DynAdd.INS_WRITE)
{
// actualize the parameters which were written into motors - and we know they were written good
C_MotorControl.ACTUALIZE_motorRegister(
pairedLastSent.rotMotor,
e_regByteType.seenValue, // as we received statusMessage after we written the value
pairedLastSent.Par);
}
else if (pairedLastSent.byteInstructionOrError == C_DynAdd.INS_READ)
{
// actualize the parameters which were read from motors
C_MotorControl.ACTUALIZE_motorRegister(
pairedLastSent.rotMotor,
e_regByteType.seenValue,
C_CONV.listOfObjects2listOfBytes(new List <object>()
{
pairedLastSent.Par[0], received.Par
})
);
}
else
{
C_SPI.LOG_debug(string.Format(
"lastSent packet: {0}\nPaired with: {1}\n{2}",
pairedLastSent.PacketBytes_toString,
received.PacketBytes_toString,
"wasn't read neither write and yet still it was supposed to be processed as a statusPacket. Not processing!"
));
}
}
else
{
LOG_statusPacket(string.Format(
"The received status packet :\t{0}\nDoes not belong to the lastSent: \t{1}",
received.PacketBytes_toString, pairedLastSent.PacketBytes_toString
));
}
}
catch (Exception e)
{
C_SPI.LOG_debug("tak tady to padá! " + e.Message);
}
}
private void txRawBytes_TextChanged(object sender, TextChangedEventArgs e)
{
if (C_State.FURTHER(e_stateProg.initialized))
{
try
{
raw = new C_Packet(C_CONV.strHex2byteArray(txRawBytes.Text, " "));
}
finally
{
btnSendRawBytes.IsEnabled = raw.IsConsistent;
}
}
}
public static void QUEUE_PacketSent(C_Packet instructionPacket)
{
int rotMot = (int)instructionPacket.rotMotor;
if (C_Packet.IS_statusPacketFollowing(instructionPacket) == true)
{
lock (queueSent_locker)
{
instructionPacket.sentTime = DateTime.Now;
queueSent[rotMot].Enqueue(instructionPacket);
queueSent_Count[rotMot] = queueSent[rotMot].Count;
C_MotorControl.ACTUALIZE_queueCounts(queueSent);
}
}
}
public static bool PROCESS_statusPacket(C_Packet status, C_Packet pairedLastSent)
{
// no echo should get here (FIND_bestPairInQueue) sort it out
if (status.IS_error() == true) // status is consistent and correct, but may have error
{
C_SPI.LOG_debug("The processed package has an error! : "
+ status.PacketBytes_toString);
}
else
{
C_SPI.LOG_debug("The processed package does not contain any error, going to process statusPacket");
ACTUALIZE_motorRegistersFromStatusPacket(status, pairedLastSent);
C_SPI.LOG_debug("Status packet processing ended");
}
return(true);
}
public static bool IS_packetChangingValue(C_Packet packet)
{
switch (packet.byteInstructionOrError)
{
case (C_DynAdd.INS_ACTION): return(true); // i think so - at least registered byte
case (C_DynAdd.INS_REG_WRITE): return(true);
case (C_DynAdd.INS_SYNC_WRITE): return(true);
case (C_DynAdd.INS_PING): return(true);
case (C_DynAdd.INS_WRITE): return(true);
case (C_DynAdd.INS_READ): return(false);
}
return(false);
//if(packet.byteId == C_DynAdd.ID_BROADCAST)
}
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++;
}
}
}
private static bool WRITE_instructionPacket(C_Packet instructionPacket)
{
openConnection.Restart();
while (openConnection.Decrement() != 0)
{
if (spi.IsOpen)
{
QUEUE_PacketSent(instructionPacket);
byte[] data = instructionPacket.PacketBytes;
LOG_cmd(data, e_cmd.sending);
WRITE_byteArray(data);
C_Packet.PROCESS_instructionPacket(instructionPacket);
return(true);
}
else
{
OPEN_connection();
}
}
return(false);
}
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);
}
}
public void SEARCH_motors()
{
// not functional yet
// should be in C_Motor
// should use background worker
// send pings and get responses - add items to [Ms] motor list
// - use local Search motor for pinging and changing of id..
byte id = C_DynAdd.ID_MIN;
C_Motor srchM = new C_Motor(id);
C_Packet querryPacket;
for (id = C_DynAdd.ID_MIN; id < C_DynAdd.ID_MAX; id++)
{
srchM.id = id;
C_SPI.spi.DiscardInBuffer();
srchM.ORDER_ping();
querryPacket = new C_Packet(srchM, C_DynAdd.INS_PING);
C_Packet.SEND_packet(querryPacket);
}
}
public static void LOG_errorByte(C_Packet packet)
{
LOG_err(packet,
GET_errorByteInfo(packet)
);
}
public static string GET_motorDataTypeInfo(C_Packet packet)
{
return(EnumGetDescription.GetDescription(packet.motorDataType));
}
public void SEND_packet(byte INSTRUCTION_BYTE, List <object> _lsParameters = null)
{
C_Packet.SEND_packet(new C_Packet(this, INSTRUCTION_BYTE, _lsParameters));
}
public static void SEND_packetToAll(byte INSTRUCTION_BYTE, List <object> _lsParameters = null)
{
C_Packet.SEND_packet(new C_Packet(INSTRUCTION_BYTE, _lsParameters));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endregion RESET
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#region static functions
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
public static void SEND_packet(C_Packet packet)
{
C_SPI.SEND_data(packet);
}
private void btnSendRawBytes_Click(object sender, RoutedEventArgs e)
{
C_Packet.SEND_packet(raw);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endregion Open close
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#region Writing
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//// I allways need to know if the command send will produce some command receive!
//public static void SEND_data(byte[] data)
//{
// SEND_data(e_cmdEcho.noEcho, data);
//}
public static void SEND_data(C_Packet instructionPacket)
{
QUEUE_PacketToSent(instructionPacket);
}
