public AHRS()
{
connected = false;
// Fill arrays used for AHRS COM
int packet_count = PName.GetValues( typeof( PName ) ).Length;
PID = new byte[packet_count];
serialPort = new SerialPort();
PacketTimer = new Timer();
int state_count = StateName.GetValues(typeof(StateName)).Length;
UpdatePending = new bool[state_count];
DataPending = new bool[state_count];
Measured = new bool[state_count];
MaxDelay = new int[state_count];
ElapsedTime = new int[state_count];
// Commands that can be sent to the AHRS device
PID[(int)PName.SET_ACTIVE_CHANNELS] = 0x80;
PID[(int)PName.SET_SILENT_MODE] = 0x81;
PID[(int)PName.SET_BROADCAST_MODE] = 0x82;
PID[(int)PName.SET_GYRO_BIAS] = 0x83;
PID[(int)PName.SET_ACCEL_BIAS] = 0x84;
PID[(int)PName.SET_ACCEL_REF_VECTOR] = 0x85;
PID[(int)PName.AUTO_SET_ACCEL_REF] = 0x86;
PID[(int)PName.ZERO_RATE_GYROS] = 0x87;
PID[(int)PName.SELF_TEST] = 0x88;
PID[(int)PName.SET_START_CAL] = 0x89;
PID[(int)PName.SET_PROCESS_COVARIANCE] = 0x8A;
PID[(int)PName.SET_MAG_COVARIANCE] = 0x8B;
PID[(int)PName.SET_ACCEL_COVARIANCE] = 0x8C;
PID[(int)PName.SET_EKF_CONFIG] = 0x8D;
PID[(int)PName.SET_GYRO_ALIGNMENT] = 0x8E;
PID[(int)PName.SET_ACCEL_ALIGNMENT] = 0x8F;
PID[(int)PName.SET_MAG_REF_VECTOR] = 0x90;
PID[(int)PName.AUTO_SET_MAG_REF] = 0x91;
PID[(int)PName.SET_MAG_CAL] = 0x92;
PID[(int)PName.SET_MAG_BIAS] = 0x93;
PID[(int)PName.SET_GYRO_SCALE] = 0x94;
PID[(int)PName.EKF_RESET] = 0x95;
PID[(int)PName.RESET_TO_FACTORY] = 0x96;
PID[(int)PName.SET_KALMAN_PARM] = 0x97;
PID[(int)PName.SET_OUTPUT] = 0x98;
PID[(int)PName.WRITE_TO_FLASH] = 0xA0;
PID[(int)PName.UPDATE_FW] = 0x9C;
PID[(int)PName.CALIBRATE_GYRO_BIAS] = 0x9B;
PID[(int)PName.GET_CONFIGURATION] = 0x9A;
PID[(int)PName.SET_REBOOT] = 0x99;
// Commands that can be sent to the AHRS device
// Commands that can be received by the AHRS class
// (ie. transmitted by the device, received by the class
// over a serial port).
// Set AHRS class parameters so that on the next call to 'synch', the class
// will attempt to acquire all internal states of the actual AHRS device.
Invalidate();
// Setup timer for keeping track of time elapsed between packet transmission and reception of response.
PacketTimer.Interval = 1; // 10 ms delay
PacketTimer.Enabled = true;
PacketTimer.Tick += new System.EventHandler(OnPacketTimerTick);
RXbuffer = new byte[RX_BUF_SIZE];
//inizialize status
m_state_covariance = new float[3, 3];
m_state_covariance[0, 0] = (float)1.0;
m_state_covariance[0, 1] = (float)0.0;
m_state_covariance[0, 2] = (float)0.0;
m_state_covariance[1, 0] = (float)0.0;
m_state_covariance[1, 1] = (float)1.0;
m_state_covariance[1, 2] = (float)0.0;
m_state_covariance[2, 0] = (float)0.0;
m_state_covariance[2, 1] = (float)0.0;
m_state_covariance[2, 2] = (float)1.0;
m_accel_alignment = new float[3, 3];
m_accel_alignment[0, 0] = (float)1.0;
m_accel_alignment[0, 1] = (float)0.0;
m_accel_alignment[0, 2] = (float)0.0;
m_accel_alignment[1, 0] = (float)0.0;
m_accel_alignment[1, 1] = (float)1.0;
m_accel_alignment[1, 2] = (float)0.0;
m_accel_alignment[2, 0] = (float)0.0;
m_accel_alignment[2, 1] = (float)0.0;
m_accel_alignment[2, 2] = (float)1.0;
m_gyro_alignment = new float[3, 3];
m_gyro_alignment[0, 0] = (float)1.0;
m_gyro_alignment[0, 1] = (float)0.0;
m_gyro_alignment[0, 2] = (float)0.0;
m_gyro_alignment[1, 0] = (float)0.0;
m_gyro_alignment[1, 1] = (float)1.0;
m_gyro_alignment[1, 2] = (float)0.0;
m_gyro_alignment[2, 0] = (float)0.0;
m_gyro_alignment[2, 1] = (float)0.0;
m_gyro_alignment[2, 2] = (float)1.0;
m_mag_cal = new float[3, 3];
m_mag_cal[0, 0] = (float)1.0;
m_mag_cal[0, 1] = (float)0.0;
m_mag_cal[0, 2] = (float)0.0;
m_mag_cal[1, 0] = (float)0.0;
m_mag_cal[1, 1] = (float)1.0;
m_mag_cal[1, 2] = (float)0.0;
m_mag_cal[2, 0] = (float)0.0;
m_mag_cal[2, 1] = (float)0.0;
m_mag_cal[2, 2] = (float)1.0;
m_init_quat = new float[4];
m_init_quat[0] = (float)0.00011;
m_init_quat[1] = (float)0.00011;
m_init_quat[2] = (float)0.00011;
m_init_quat[3] = (float)0.00011;
m_Q_bias = new float[3];
m_Q_bias[0] = (float)0.0000795;
m_Q_bias[1] = (float)0.00007925;
m_Q_bias[2] = (float)0.00007925;
m_gain = new float[4];
m_gain[0] = (float)0.00008;
m_gain[1] = (float)0.0;
m_gain[2] = (float)0.0;
m_gain[3] = (float)0.0;
m_filter_type= (float)0.0;
m_convolution_time= (float)0.0;
m_y_gyro_bias= (float)0.0;
m_z_gyro_bias= (float)0.0;
m_x_accel_bias= (float)0.0;
m_y_accel_bias= (float)0.0;
m_z_accel_bias= (float)0.0;
m_x_mag_bias= (float)0.0;
m_y_mag_bias= (float)0.0;
m_z_mag_bias= (float)0.0;
m_mag_covariance= (float)0.0025;
m_accel_covariance = (float)0.000799;
m_output_enable = (UInt16)0;
m_output_rate= (UInt16)200;
m_badurate= (UInt16)1;
m_port= (UInt16)1200;
m_ip = new char[30];
String str = "192.168.010.030";
m_ip = str.ToCharArray();
// confReceivedEvent(1);
}
