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