private void ClearState(bool validate, string reason = "")
{
if (!validate)
{
Debug("[DropByte:" + reason + "]" + byteToIntStr(CommandBytes.ToArray()));
}
else
{
//Debug("[Validate]"+byteToIntStr (CommandBytes.ToArray ()));
}
HEADER_INDEX = 0;
DEMANDING_SIZE = 0;
DATA_INDEX = 0;
COMMAND_TYPE = 0;
CommandBytes.Clear();
ReceivingState = MultiWiiState.WAITING_PRENABLE_HEADER;
}
private void ByteParsing(byte newByte)
{
switch (ReceivingState)
{
//step 0
case MultiWiiState.WAITING_PRENABLE_HEADER:
if ((newByte) == PREAMBLE_HEADER[HEADER_INDEX])
{
HEADER_INDEX++;
CommandBytes.Add(newByte);
if (HEADER_INDEX == PREAMBLE_HEADER.Length)
{
ReceivingState = MultiWiiState.WAITING_DIRECTION;
}
}
else
{
CommandBytes.Add(newByte);
ClearState(false, "INVALIDATE_HEADER");
}
break;
//step 1
case MultiWiiState.WAITING_DIRECTION:
if (((char)newByte) == DIRECTION_FROM_DRONE)
{
CommandBytes.Add(newByte);
ReceivingState = MultiWiiState.WAITING_SIZE;
}
else
{
CommandBytes.Add(newByte);
ClearState(false, "DIRECTION_FLAG_ERROR");
}
break;
//step 2
case MultiWiiState.WAITING_SIZE:
CommandBytes.Add(newByte);
DEMANDING_SIZE = (int)newByte;
ReceivingState = MultiWiiState.WAITING_COMMAND_TYPE;
break;
//step 3
case MultiWiiState.WAITING_COMMAND_TYPE:
CommandBytes.Add(newByte);
COMMAND_TYPE = (int)newByte;
if (DEMANDING_SIZE == 0)
{
ReceivingState = MultiWiiState.WAITING_CRC;
}
else
{
ReceivingState = MultiWiiState.WAITING_DATA;
}
break;
//step 4
case MultiWiiState.WAITING_DATA:
CommandBytes.Add(newByte);
DATA_INDEX++;
if (DATA_INDEX == DEMANDING_SIZE)
{
ReceivingState = MultiWiiState.WAITING_CRC;
}
break;
case MultiWiiState.WAITING_CRC:
byte sum = 0;
//int len = CommandBytes.to
//check sum for size(3),commandType(4),data(5~)
for (int i = CHECK_SUM_START_INDEX; i < CommandBytes.Count; i++)
{
sum = (byte)(sum ^ CommandBytes[i]);
}
//validate
if (sum == newByte)
{
CommandBytes.Add(newByte);
RunCommand(CommandBytes);
ClearState(true);
}
else
{
CommandBytes.Add(newByte);
ClearState(false, "CRC_NOT_VALIDATE");
}
break;
}
}
