private void handle_packet(byte type, int length, byte[] data)
{
int act_channels = 0xFFFF;
int i = 4;
byte[] Data_tmp=new byte[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];
m_recentData[Q1_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_q1 = (float)m_recentData[Q1_INDEX];
i += 4;
// Pitch angle
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Q2_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_q2 = (float)m_recentData[Q2_INDEX];
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];
m_recentData[Q3_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_q3 = (float)m_recentData[Q3_INDEX];
i += 4;
// quarto campo
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Q4_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_q4 = (float)m_recentData[Q4_INDEX];
i += 4;
// Accel X
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[X_ACCEL_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_accelX = (float)m_recentData[X_ACCEL_INDEX];
i += 4;
// Accel Y
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Y_ACCEL_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_accelY = (float)m_recentData[Y_ACCEL_INDEX];
i += 4;
// Accel Z
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Z_ACCEL_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_accelZ = (float)m_recentData[Z_ACCEL_INDEX];
i += 4;
// gyro_x
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[X_GYRO_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_gyroX = (float)m_recentData[X_GYRO_INDEX];
i += 4;
// gyro_y
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Y_GYRO_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_gyroY = (float)m_recentData[Y_GYRO_INDEX];
i += 4;
// gyro_z
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Z_GYRO_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_gyroZ = (float)m_recentData[Z_GYRO_INDEX];
i += 4;
// Mag X
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[X_MAG_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_magX = (float)m_recentData[X_MAG_INDEX];
i += 4;
// Mag Y
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Y_MAG_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_magY = (float)m_recentData[Y_MAG_INDEX];
i += 4;
// Mag Z
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[Z_MAG_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_magZ = (float)m_recentData[Z_MAG_INDEX];
i += 4;
// latitude
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[LATITUDE_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_latitude = (float)m_recentData[LATITUDE_INDEX];
i += 4;
// longitude
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[LONGITUDE_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_longitude = (float)m_recentData[LONGITUDE_INDEX];
i += 4;
// vel_gps
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[VEL_GPS_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_velGPS = (float)m_recentData[VEL_GPS_INDEX];
i += 4;
// altitudine
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[ALTITUDINE_INDEX] = BitConverter.ToSingle(Data_tmp, 0);
m_altitudine = (float)m_recentData[ALTITUDINE_INDEX];
i += 4;
// satellite_number
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[SATELLITE_NUMBER_INDEX] = (float)BitConverter.ToInt32(Data_tmp, 0);
m_satellite_number= (float)m_recentData[SATELLITE_NUMBER_INDEX];
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];
m_recentData[ID_DISP] = (float)BitConverter.ToInt32(Data_tmp, 0);
m_ID_DISP = (float)m_recentData[ID_DISP];
i += 4;
// crc
Data_tmp[0] = data[i + 0];
Data_tmp[1] = data[i + 1];
Data_tmp[2] = data[i + 2];
Data_tmp[3] = data[i + 3];
m_recentData[CRC_INDEX] = (float)BitConverter.ToInt32(Data_tmp, 0);
m_crc= (float)m_recentData[CRC_INDEX];
i += 4;
m_q1s = m_q1;
m_q2s = m_q2;
m_q3s = m_q3;
m_q4s = m_q4;
float[] Quaternion = new float[] { (float)m_q1, (float)m_q2, (float)m_q3, (float)m_q4 };
float pitch, roll,yaw;
double phi2 ;
double theta2;
double psi2 ;
float z=Quaternion[0];
float y=Quaternion[1];
float x=Quaternion[2];
float w=Quaternion[3];
// Used to determine if near vertical where asin() function is not
//defined.
double ASIN_LIMIT = 0.9999939;
yaw = (float)Math.Atan2(2 * (x * y + z * w), 2 * w * w + 2 * x * x - 1);
pitch = 2 * (x * z - y * w);
roll = 0; //roll has no meaning if vertical
if (pitch < ASIN_LIMIT)
{
if (pitch > -ASIN_LIMIT)
{
roll = (float)Math.Atan2(2 * (y * z + x * w), 2 * w * w + 2 * z * z - 1);
pitch = 2 * (x * z - y * w);
pitch = -(float)Math.Asin(pitch);
}
else
{
pitch = -(float)Math.PI / 2;
}
}
else
{
pitch =(float)Math.PI / 2;
}
m_rollAngle = yaw * 57.2957795130823;
m_pitchAngle = -pitch * 57.2957795130823;
m_yawAngle = -roll *57.2957795130823;
roll = -roll;
phi2 = (+yaw * Math.Cos(roll) + pitch * Math.Sin(roll)) / 2.0;
theta2 = (-yaw * Math.Sin(roll) + pitch * Math.Cos(roll)) / 2.0;
psi2 = roll / 2.0;// yaw / 2.0;
double sinphi2 = Math.Sin(phi2);
double cosphi2 = Math.Cos(phi2);
double sintheta2 = Math.Sin(theta2);
double costheta2 = Math.Cos(theta2);
double sinpsi2 = Math.Sin(psi2);
double cospsi2 = Math.Cos(psi2);
double cba = 0;//m_q1;
double cbb = 0;//m_q2;
double cbc = 0;//m_q3;
double cbd = 0;//m_q4;
cba = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
cbb = sinphi2 * costheta2 * cospsi2 - cosphi2 * sintheta2 * sinpsi2;
cbc = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
cbd = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
m_q1 = cba;
m_q2 = cbb;
m_q3 = cbc;
m_q4 = cbd;
DataReceivedEvent(act_channels);
}
