private bool updateAHRS(int index)
{
Packet AHRSPacket = new Packet();
byte_conversion_array ftob = new byte_conversion_array();
if (index == (int)StateName.ACTIVE_CHANNELS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACTIVE_CHANNELS];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
int active_channels = 0;
if (m_accelZ_active)
active_channels |= (1 << 1);
if (m_accelY_active)
active_channels |= (1 << 2);
if (m_accelX_active)
active_channels |= (1 << 3);
if (m_gyroZ_active)
active_channels |= (1 << 4);
if (m_gyroY_active)
active_channels |= (1 << 5);
if (m_gyroX_active)
active_channels |= (1 << 6);
if (m_magZ_active)
active_channels |= (1 << 7);
if (m_magY_active)
active_channels |= (1 << 8);
if (m_magX_active)
active_channels |= (1 << 9);
if (m_latitude_active)
active_channels |= (1 << 10);
if (m_longitude_active)
active_channels |= (1 << 11);
if (m_velGPS_active)
active_channels |= (1 << 12);
if (m_altitudine_active)
active_channels |= (1 << 13);
if (m_satellite_number_active)
active_channels |= (1 << 14);
AHRSPacket.Data[0] = (byte)((active_channels >> 8) & 0x0FF);
AHRSPacket.Data[1] = (byte)((active_channels) & 0x0FF);
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_ALIGNMENT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_ALIGNMENT];
AHRSPacket.DataLength = 36;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_accel_alignment[0, 0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_accel_alignment[0, 1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_accel_alignment[0, 2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_accel_alignment[1, 0];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_accel_alignment[1, 1];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_accel_alignment[1, 2];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_accel_alignment[2, 0];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_accel_alignment[2, 1];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_accel_alignment[2, 2];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_COVARIANCE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_COVARIANCE];
AHRSPacket.DataLength = 4;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_accel_covariance;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_BIAS];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_accel_bias;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_accel_bias;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_accel_bias;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_REF)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_REF_VECTOR];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_accel_ref;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_accel_ref;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_accel_ref;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_BIAS];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_mag_bias;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_mag_bias;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_mag_bias;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_REF)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_REF_VECTOR];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_mag_ref;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_mag_ref;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_mag_ref;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.BROADCAST_MODE)
{
if (m_broadcast_enabled)
{
AHRSPacket.PacketType = PID[(int)PName.SET_BROADCAST_MODE];
AHRSPacket.DataLength = 1;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
AHRSPacket.Data[0] = (byte)(m_broadcast_rate);
}
else
{
AHRSPacket.PacketType = PID[(int)PName.SET_SILENT_MODE];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
}
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.EKF_CONFIG)
{
AHRSPacket.PacketType = PID[(int)PName.SET_EKF_CONFIG];
AHRSPacket.DataLength = 1;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
byte data = 0;
if (m_accel_enabled)
data |= 0x02;
if (m_mag_enabled)
data |= 0x01;
if (m_mag_restriced_to_yaw)
data |= 0x04;
AHRSPacket.Data[0] = data;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GYRO_ALIGNMENT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_GYRO_ALIGNMENT];
AHRSPacket.DataLength = 36;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_gyro_alignment[0, 0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_gyro_alignment[0, 1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_gyro_alignment[0, 2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_gyro_alignment[1, 0];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_gyro_alignment[1, 1];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_gyro_alignment[1, 2];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_gyro_alignment[2, 0];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_gyro_alignment[2, 1];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_gyro_alignment[2, 2];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GYRO_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_GYRO_BIAS];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_gyro_bias;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_gyro_bias;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_gyro_bias;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GYRO_SCALE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_GYRO_SCALE];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_z_gyro_scale;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_gyro_scale;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_x_gyro_scale;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_COVARIANCE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_COVARIANCE];
AHRSPacket.DataLength = 4;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_mag_covariance;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.PROCESS_COVARIANCE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_PROCESS_COVARIANCE];
AHRSPacket.DataLength = 4;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_process_covariance;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.START_CAL)
{
AHRSPacket.PacketType = PID[(int)PName.SET_START_CAL];
AHRSPacket.DataLength = 1;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
if (m_zero_gyros_on_startup)
AHRSPacket.Data[0] = 0x01;
else
AHRSPacket.Data[0] = 0x00;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ZERO_GYROS)
{
AHRSPacket.PacketType = PID[(int)PName.ZERO_RATE_GYROS];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.AUTO_SET_MAG_REF)
{
AHRSPacket.PacketType = PID[(int)PName.AUTO_SET_MAG_REF];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.AUTO_SET_ACCEL_REF)
{
AHRSPacket.PacketType = PID[(int)PName.AUTO_SET_ACCEL_REF];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.SELF_TEST)
{
AHRSPacket.PacketType = PID[(int)PName.SELF_TEST];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.RESET_TO_FACTORY)
{
AHRSPacket.PacketType = PID[(int)PName.RESET_TO_FACTORY];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.EKF_RESET)
{
AHRSPacket.PacketType = PID[(int)PName.EKF_RESET];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.REBOOT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_REBOOT];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_CAL)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_CAL];
AHRSPacket.DataLength = 36;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_mag_cal[0, 0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_mag_cal[0, 1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_mag_cal[0, 2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_mag_cal[1, 0];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_mag_cal[1, 1];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_mag_cal[1, 2];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_mag_cal[2, 0];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_mag_cal[2, 1];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_mag_cal[2, 2];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.SET_KALMAN_PARM)
{
AHRSPacket.PacketType = PID[(int)PName.SET_KALMAN_PARM];
AHRSPacket.DataLength = 52;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_init_quat[0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_init_quat[1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_init_quat[2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_init_quat[3];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_Q_bias[ 0];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_Q_bias[1];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_Q_bias[2];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_gain[0];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_gain[1];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
ftob.float0 = m_gain[2];
AHRSPacket.Data[36] = ftob.byte0;
AHRSPacket.Data[37] = ftob.byte1;
AHRSPacket.Data[38] = ftob.byte2;
AHRSPacket.Data[39] = ftob.byte3;
ftob.float0 = m_gain[3];
AHRSPacket.Data[40] = ftob.byte0;
AHRSPacket.Data[41] = ftob.byte1;
AHRSPacket.Data[42] = ftob.byte2;
AHRSPacket.Data[43] = ftob.byte3;
ftob.float0 = m_filter_type;
AHRSPacket.Data[44] = ftob.byte0;
AHRSPacket.Data[45] = ftob.byte1;
AHRSPacket.Data[46] = ftob.byte2;
AHRSPacket.Data[47] = ftob.byte3;
ftob.float0 = m_convolution_time;
AHRSPacket.Data[48] = ftob.byte0;
AHRSPacket.Data[49] = ftob.byte1;
AHRSPacket.Data[50] = ftob.byte2;
AHRSPacket.Data[51] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.SET_OUTPUT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_OUTPUT];
AHRSPacket.DataLength = 24;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
AHRSPacket.Data[0] = (byte)((m_output_enable >> 8) & 0x0FF);
AHRSPacket.Data[1] = (byte)(m_output_enable & 0x0FF);
AHRSPacket.Data[2] = (byte)((m_output_rate >> 8) & 0x0FF);
AHRSPacket.Data[3] = (byte)(output_rate & 0x0FF);
AHRSPacket.Data[4] = (byte)((m_badurate >> 8) & 0x0FF);
AHRSPacket.Data[5] = (byte)(m_badurate & 0x0FF);
AHRSPacket.Data[6] = (byte)((m_port >> 8) & 0x0FF);
AHRSPacket.Data[7] = (byte)(m_port & 0x0FF);
for(int i =0; i<=14; i ++)
{
if (i <= (m_ip.Length-1))
AHRSPacket.Data[i+8] = (byte)m_ip[i];
else
AHRSPacket.Data[i+8]=(byte)' ' ;
}
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.WRITE_TO_FLASH)
{
AHRSPacket.PacketType = PID[(int)PName.WRITE_TO_FLASH];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.CALIBRATE_GYRO_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.CALIBRATE_GYRO_BIAS];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.UPDATE_FW)
{
AHRSPacket.PacketType = PID[(int)PName.UPDATE_FW];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GET_CONFIGURATION)
{
AHRSPacket.PacketType = PID[(int)PName.GET_CONFIGURATION];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
return true;
}
