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