/// <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.
}
/// <summary>
/// Send command to quadcopter.
/// <para>
/// <list>
/// Protocol:
/// #CMD#: 1 byte
/// 'C' : SETCMD
/// CMD: 1 byte
/// 0 -- DISARM
/// 1 -- BLINK
/// 2 -- X-MODE
/// 3 -- P-MODE
/// 4 -- CALIBRATE_ACC
///
/// DataValue: 6
/// CHKSUM: 1 byte
/// </list>
/// </para>
/// </summary>
/// <param name="CMD_ID"></param>
public HefnyCopterCommand(ENUM_SerialCommands CMD_ID)
{
mCommandArray[0] = (byte) 'C'; // Execute Action Command
mCommandArray[1] = (byte)CMD_ID;
}
public virtual void SendCommand(ENUM_SerialCommands SerialCommand)
{
SendCommand(new HefnyCopterCommand(SerialCommand));
}
