/*
* 0 M0V
* 1 M1V
* 2 M2V
* 3 M3V
* 4 MOA
* 5 M1A
* 6 M2A
* 7 M3A
* 8 M0T
* 9 M1T
* 10 M2T
* 11 M3T
* 12 M0TH
* 13 M1TH
* 14 M2TH
* 15 M3TH
* 16 A0V
* 17 A0A
* 18 A0T
* 19 A1V
* 20 A1A
* 21 A1T
* 22 G0X
* 23 G0Y
* 25 G0Z
*/
//Converts the bytes into an object of type Data
/// <summary>
/// TODO Fix that the data is setup in the singleton classes the data gets stored in our
/// variables in this class so...
///
/// opcodes:
/// reject = 0
/// ok =1
/// sync =2
/// report =3
/// keepalive =4
///
///
/// </summary>
/// <param name="data"></param>
public int GetPackage(byte[] data)
{
int return_opcode = 0;
try
{
opcode = data[0];
}
catch {
Console.WriteLine("unable to read opcode");
}
if (opcode == 0)
{
//data reject
return_opcode = 0;
}
if (opcode == 1)
{
//data ok
return_opcode = 1;
}
else if (opcode == 2)
{
//data sync
try
{
//
SyncStructPackage pack = new SyncStructPackage();
unsafe
{
SyncStructPackage* p;
GCHandle gch = GCHandle.Alloc(data, GCHandleType.Pinned);
p = (SyncStructPackage*)gch.AddrOfPinnedObject().ToPointer();
pack = *p;
gch.Free();
}
DateTime this_time = DateTime.Now;
Console.WriteLine("succesfully synced");
Console.WriteLine(pack.ToString());
Receive_Singleton.Instance.Current_ses.Sensors[0].AddValueDataToArray(new ValueData(pack.voltage_motor_0, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[1].AddValueDataToArray(new ValueData(pack.voltage_motor_1, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[2].AddValueDataToArray(new ValueData(pack.voltage_motor_2, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[3].AddValueDataToArray(new ValueData(pack.voltage_motor_3, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[4].AddValueDataToArray(new ValueData(pack.current_motor_0, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[5].AddValueDataToArray(new ValueData(pack.current_motor_1, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[6].AddValueDataToArray(new ValueData(pack.current_motor_2, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[7].AddValueDataToArray(new ValueData(pack.current_motor_3, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[8].AddValueDataToArray(new ValueData(pack.Temp_0, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[9].AddValueDataToArray(new ValueData(pack.Temp_1, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[10].AddValueDataToArray(new ValueData(pack.Temp_2, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[11].AddValueDataToArray(new ValueData(pack.Temp_3, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[12].AddValueDataToArray(new ValueData(pack.throttle_0, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[13].AddValueDataToArray(new ValueData(pack.throttle_1, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[14].AddValueDataToArray(new ValueData(pack.throttle_2, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[15].AddValueDataToArray(new ValueData(pack.throttle_3, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[16].AddValueDataToArray(new ValueData(pack.accu_voltage, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[17].AddValueDataToArray(new ValueData(pack.accu_current, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[18].AddValueDataToArray(new ValueData(pack.accu_temp1, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[19].AddValueDataToArray(new ValueData(pack.accu_voltage, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[20].AddValueDataToArray(new ValueData(pack.accu_current, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[21].AddValueDataToArray(new ValueData(pack.accu_temp1, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[22].AddValueDataToArray(new ValueData(pack.gyro_x, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[23].AddValueDataToArray(new ValueData(pack.gyro_y, this_time));
Receive_Singleton.Instance.Current_ses.Sensors[24].AddValueDataToArray(new ValueData(pack.gyro_z, this_time));
//Receive_Singleton.Instance.Current_ses.Sensors[25].AddValueDataToArray(new ValueData(pack.voltage_motor_0, this_time));
}
catch (Exception e)
{
Console.WriteLine("het lezen van syncpackage is mislukt");
}
return_opcode = 2;
}
//get list sounds "REPORT"
else if (opcode == 3)
{
int pad = data[1];
int protocol = data[2];
int revision = data[3];
int list_count = data[4]; //get total numbers of words
words = new string[list_count];//create the length of an array
int counter_word = 0; //wich word on what place in an array
bool new_word = true; //if there is an new word get the new length
int length_word = 0;
for (int i = 5; i < list_count; i++) // total list length
{
if (new_word)//get new length of new word
{
length_word = data[i];// if correct the length is on this position
new_word = false; //stop doing that!
}
char[] chars = new char[length_word];//we now our length now
for (int j = 0; j < length_word; j++)//for each char in the length of word
{
chars[j] = BitConverter.ToChar(data, i);
i++;
}
words[counter_word] = chars.ToString();
counter_word++;
new_word = true;
}
return_opcode = 3;
}
else if (opcode == 4)
{
//keep alive
try
{
//
keepaliveStructPackage keepalive = new keepaliveStructPackage();
unsafe
{
keepaliveStructPackage* p;
GCHandle gch = GCHandle.Alloc(data, GCHandleType.Pinned);
p = (keepaliveStructPackage*)gch.AddrOfPinnedObject().ToPointer();
keepalive = *p;
gch.Free();
Console.WriteLine("Read Keep alive package");
}
}
catch (Exception e)
{
Console.WriteLine("Unable to read keap alive");
Console.WriteLine(e);
}
return_opcode = 4;
}
else { return_opcode = 99; }
Console.WriteLine("0==reject 1==ok 2==sync 3==report 4==keepalive");
Console.WriteLine("received kinda package : " + return_opcode);
return return_opcode;
}
/// <summary>
/// Standard keep alive package
/// </summary>
public keepaliveStructPackage keep()
{
keepaliveStructPackage keep_alive = new keepaliveStructPackage();
keep_alive.opcode = 4;
keep_alive.padding = 1;
return keep_alive;
}
