private void handle_packet_configuration(byte type, int length, byte[] data)
{
int i = 4;
// int endid=0;
byte[] Data_tmp = new byte[4];
float[] data_tmp_acc_alig = new float [9];
float[] data_tmp_acc_bias = new float[3];
float[] data_tmp_mag_alig = new float[9];
float[] data_tmp_mag_bias = new float[3];
float[] data_tmp_gyro_alig = new float[9];
float[] data_tmp_gyro_bias = new float[3];
float acc_covariance = 0;
float mag_covariance = 0;
float[] q_biasr = new float[4];
float[] quat_initr = new float[4];
float[] gainr = new float[4];
float filter_typer = 0;
float convolution_timer = 0;
int output_enabler = 0;
int output_rater = 0;
int badurater = 0;
int portr = 0;
int id_dispositivo = 0;
int FW_version = 0;
//acc
configuration_String="";
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[3] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[4] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[5] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[6] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[7] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_alig[8] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_bias[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_bias[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_acc_bias[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//mag
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[3] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[4] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[5] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[6] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[7] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_alig[8] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_bias[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_bias[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_mag_bias[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//gyro
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[3] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[4] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[5] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[6] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[7] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_alig[8] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_bias[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_bias[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
data_tmp_gyro_bias[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//acc covariance
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
acc_covariance = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//mag covariance
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
mag_covariance = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//quat init
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
quat_initr[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
quat_initr[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
quat_initr[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
quat_initr[3] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//qbias init
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
q_biasr[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
q_biasr[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
q_biasr[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//gain init
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
gainr[0] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
gainr[1] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
gainr[2] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
gainr[3] = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//filter_type_parser
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
filter_typer = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//convolution_time_parser
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
convolution_timer = BitConverter.ToSingle(Data_tmp, 0);
i += 4;
//output_enabler
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
output_enabler = BitConverter.ToInt32(Data_tmp, 0);
i += 4;
//output_rater
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
output_rater = BitConverter.ToInt32(Data_tmp, 0);
i += 4;
//badurater
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
badurater = BitConverter.ToInt32(Data_tmp, 0);
i += 4;
//portr
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
portr = BitConverter.ToInt32(Data_tmp, 0);
i += 4;
//id_dispositivo
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
id_dispositivo = BitConverter.ToInt32(Data_tmp, 0);
i += 4;
//FW_version
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
FW_version = BitConverter.ToInt32(Data_tmp, 0);
configuration_String += "Id ";
configuration_String += id_dispositivo.ToString();
configuration_String += "\n\n";
configuration_String += "FW_version ";
configuration_String += FW_version.ToString();
configuration_String += "\n\n";
configuration_String += "acc alignment \n";
for (int j = 0; j <=8 ; j++)
{
configuration_String += data_tmp_acc_alig[j].ToString();
configuration_String += " ";
if (j == 2 || j == 5)
{
configuration_String += "\n";
}
}
configuration_String += "\n\n";
configuration_String += "acc bias \n";
for (int j = 0; j <=2; j++)
{
configuration_String += data_tmp_acc_bias[j].ToString();
configuration_String += " ";
}
configuration_String += "\n\n";
configuration_String += "mag alignment \n";
for (int j = 0; j <= 8; j++)
{
configuration_String += data_tmp_mag_alig[j].ToString();
configuration_String += " ";
if (j == 2 || j == 5)
{
configuration_String += "\n";
}
}
configuration_String += "\n\n";
configuration_String += "mag bias \n";
for (int j = 0; j <=2; j++)
{
configuration_String += data_tmp_mag_bias[j].ToString();
configuration_String += " ";
}
configuration_String += "\n\n";
configuration_String += "gyro alignment \n";
for (int j = 0; j <= 8; j++)
{
configuration_String += data_tmp_gyro_alig[j].ToString();
configuration_String += " ";
if (j == 2 || j == 5)
{
configuration_String += "\n";
}
}
configuration_String += "\n\n";
configuration_String += "gyro bias \n";
for (int j = 0; j <=2; j++)
{
configuration_String += data_tmp_gyro_bias[j].ToString();
configuration_String += " ";
}
configuration_String += "\n\n";
configuration_String += "q_bias \n";
for (int j = 0; j <=2; j++)
{
configuration_String += q_biasr[j].ToString();
configuration_String += " ";
}
configuration_String += "\n\n";
configuration_String += "quat_init \n";
for (int j = 0; j <=3; j++)
{
configuration_String += quat_initr[j].ToString();
configuration_String += " ";
}
configuration_String += "\n\n";
configuration_String += "gain \n";
for (int j = 0; j <=3; j++)
{
configuration_String += gainr[j].ToString();
configuration_String += " ";
}
configuration_String += "\n\n";
configuration_String += "acc_covariance ";
configuration_String += acc_covariance.ToString();
configuration_String += "\n";
configuration_String += "mag_covariance ";
configuration_String += mag_covariance.ToString();
configuration_String += "\n";
configuration_String += "filter_type ";
configuration_String += filter_typer.ToString();
configuration_String += "\n";
configuration_String += "convolution_time ";
configuration_String += convolution_timer.ToString();
configuration_String += "\n";
configuration_String += "output_enable ";
configuration_String += output_enabler.ToString();
configuration_String += "\n";
configuration_String += "output_rate ";
configuration_String += output_rater.ToString();
configuration_String += "\n";
configuration_String += "badurate ";
configuration_String += badurater.ToString();
configuration_String += "\n";
configuration_String += "port ";
configuration_String += portr.ToString();
configuration_String += "\n";
}
