using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using System.IO;
using System.IO.Ports;
using System.Runtime.InteropServices;

namespace AHRSInterface
{
    // Delegate types for interfacing with AHRS class events
    // The PacketDelegate type is used to create handlers for 'PacketReceived' events
    // and for 'COMFailed' events.
    public delegate void PacketDelegate(PName packet_type, int flags);

    public delegate void StateDelegate(StateName state_type, int flags);

    // The DataReceivedDelegate is used to create handlers for 'DataReceived' events
    public delegate void DataReceivedDelegate(int active_channels);

    public delegate void COMFailedDelegate();

    // Define a struct for converting a 4-byte data sequences into different data types
    [StructLayout(LayoutKind.Explicit)]
    struct byte_conversion_array
    {
        // Four Bytes
        [FieldOffset(0)]
        public byte byte3;

        [FieldOffset(1)]
        public byte byte2;

        [FieldOffset(2)]
        public byte byte1;

        [FieldOffset(3)]
        public byte byte0;

        // One Float
        [FieldOffset(0)]
        public float float0;

        // One 32-bit integer
        [FieldOffset(0)]
        public Int32 int32;

        // Two 16-bit integers
        [FieldOffset(0)]
        public Int16 int16_0;

        [FieldOffset(2)]
        public Int16 int16_1;

        // Two 16-bit unsigned integers
        [FieldOffset(0)]
        public UInt16 uint16_0;

        [FieldOffset(2)]
        public UInt16 uint16_1;
    }

    public enum PName
    {
        // TX Packet names (packets that can be received by the AHRS class)
        SET_ACTIVE_CHANNELS = 0,
        SET_SILENT_MODE,
        SET_BROADCAST_MODE,
        
        SET_GYRO_BIAS,
        SET_GYRO_SCALE,
        SET_GYRO_ALIGNMENT,
        ZERO_RATE_GYROS,
        SET_START_CAL,
        
        SET_ACCEL_BIAS,
        SET_ACCEL_ALIGNMENT,
        SET_ACCEL_REF_VECTOR,
        AUTO_SET_ACCEL_REF,

        SET_MAG_CAL,
        SET_MAG_BIAS,
        SET_MAG_REF_VECTOR,
        AUTO_SET_MAG_REF,

        EKF_RESET,
        SET_EKF_CONFIG,
        SET_PROCESS_COVARIANCE,
        SET_MAG_COVARIANCE,
        SET_ACCEL_COVARIANCE,
        SELF_TEST,

        RESET_TO_FACTORY,
        WRITE_TO_FLASH,

        GET_DATA,
        GET_ACTIVE_CHANNELS,
        GET_BROADCAST_MODE,
        GET_ACCEL_BIAS,
        GET_ACCEL_REF_VECTOR,
        GET_GYRO_BIAS,
        GET_GYRO_SCALE,
        GET_START_CAL,
        GET_EKF_CONFIG,
        GET_PROCESS_COVARIANCE,
        GET_MAG_COVARIANCE,
        GET_MAG_REF_VECTOR,
        GET_MAG_CAL,
        GET_MAG_BIAS,
        GET_ACCEL_COVARIANCE,
        GET_STATE_COVARIANCE,
        GET_ACCEL_ALIGNMENT,
        GET_GYRO_ALIGNMENT,

        // Packets that can be received by the AHRS class
        COMMAND_COMPLETE,
        COMMAND_FAILED,
	    BAD_CHECKSUM,
	    BAD_DATA_LENGTH,
	    UNRECOGNIZED_PACKET,
	    BUFFER_OVERFLOW,
	    STATUS_REPORT,
	    SENSOR_DATA,
	    GYRO_BIAS_REPORT,
	    GYRO_SCALE_REPORT,
	    START_CAL_REPORT,
	    ACCEL_BIAS_REPORT,
	    ACTIVE_CHANNEL_REPORT,
	    ACCEL_COVARIANCE_REPORT,
        ACCEL_REF_VECTOR_REPORT,
        MAG_REF_VECTOR_REPORT,
	    MAG_COVARIANCE_REPORT,
	    PROCESS_COVARIANCE_REPORT,
	    STATE_COVARIANCE_REPORT,
	    EKF_CONFIG_REPORT,
	    GYRO_ALIGNMENT_REPORT,
        ACCEL_ALIGNMENT_REPORT,
        MAG_CAL_REPORT,
        MAG_BIAS_REPORT,
	    BROADCAST_MODE_REPORT
    }

    public enum StateName
    {
        ACTIVE_CHANNELS,
        BROADCAST_MODE,
        ACCEL_BIAS,
        ACCEL_ALIGNMENT,
        ACCEL_COVARIANCE,
        ACCEL_REF,
        AUTO_SET_ACCEL_REF,
        GYRO_BIAS,
        GYRO_SCALE,
        GYRO_ALIGNMENT,
        START_CAL,
        EKF_CONFIG,
        PROCESS_COVARIANCE,
        MAG_COVARIANCE,
        MAG_CAL,
        MAG_BIAS,
        MAG_REF,
        AUTO_SET_MAG_REF,
        ZERO_GYROS,
        WRITE_TO_FLASH,
        RESET_TO_FACTORY,
        SELF_TEST,
        EKF_RESET,
        SENSOR_DATA,
        STATE_COVARIANCE
    }

    public class AHRS
    {
        // Default constructor
        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.WRITE_TO_FLASH] = 0xA0;

            PID[(int)PName.GET_DATA] = 0x01;
            PID[(int)PName.GET_ACTIVE_CHANNELS] = 0x02;
            PID[(int)PName.GET_BROADCAST_MODE] = 0x03;
            PID[(int)PName.GET_ACCEL_BIAS] = 0x04;
            PID[(int)PName.GET_ACCEL_REF_VECTOR] = 0x05;
            PID[(int)PName.GET_GYRO_BIAS] = 0x06;
            PID[(int)PName.GET_GYRO_SCALE] = 0x07;
            PID[(int)PName.GET_START_CAL] = 0x08;
            PID[(int)PName.GET_EKF_CONFIG] = 0x09;
            PID[(int)PName.GET_ACCEL_COVARIANCE] = 0x0A;
            PID[(int)PName.GET_MAG_COVARIANCE] = 0x0B;
            PID[(int)PName.GET_PROCESS_COVARIANCE] = 0x0C;
            PID[(int)PName.GET_STATE_COVARIANCE] = 0x0D;
            PID[(int)PName.GET_GYRO_ALIGNMENT] = 0x0E;
            PID[(int)PName.GET_ACCEL_ALIGNMENT] = 0x0F;
            PID[(int)PName.GET_MAG_REF_VECTOR] = 0x10;
            PID[(int)PName.GET_MAG_CAL] = 0x11;
            PID[(int)PName.GET_MAG_BIAS] = 0x12;

            // Commands that can be received by the AHRS class
            // (ie. transmitted by the device, received by the class
            // over a serial port).
            PID[(int)PName.COMMAND_COMPLETE] = 0xB0;
            PID[(int)PName.COMMAND_FAILED] = 0xB1;
            PID[(int)PName.BAD_CHECKSUM] = 0xB2;
            PID[(int)PName.BAD_DATA_LENGTH] = 0xB3;
            PID[(int)PName.UNRECOGNIZED_PACKET] = 0xB4;
            PID[(int)PName.BUFFER_OVERFLOW] = 0xB5;
            PID[(int)PName.STATUS_REPORT] = 0xB6;
            PID[(int)PName.SENSOR_DATA] = 0xB7;
            PID[(int)PName.GYRO_BIAS_REPORT] = 0xB8;
            PID[(int)PName.GYRO_SCALE_REPORT] = 0xB9;
            PID[(int)PName.START_CAL_REPORT] = 0xBA;
            PID[(int)PName.ACCEL_BIAS_REPORT] = 0xBB;
            PID[(int)PName.ACCEL_REF_VECTOR_REPORT] = 0xBC;
            PID[(int)PName.ACTIVE_CHANNEL_REPORT] = 0xBD;
            PID[(int)PName.ACCEL_COVARIANCE_REPORT] = 0xBE;
            PID[(int)PName.MAG_COVARIANCE_REPORT] = 0xBF;
            PID[(int)PName.PROCESS_COVARIANCE_REPORT] = 0xC0;
            PID[(int)PName.STATE_COVARIANCE_REPORT] = 0xC1;
            PID[(int)PName.EKF_CONFIG_REPORT] = 0xC2;
            PID[(int)PName.GYRO_ALIGNMENT_REPORT] = 0xC3;
            PID[(int)PName.ACCEL_ALIGNMENT_REPORT] = 0xC4;
            PID[(int)PName.MAG_REF_VECTOR_REPORT] = 0xC5;
            PID[(int)PName.MAG_CAL_REPORT] = 0xC6;
            PID[(int)PName.MAG_BIAS_REPORT] = 0xC7;
            PID[(int)PName.BROADCAST_MODE_REPORT] = 0xC8;

            // 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 = 50;      // 10 ms delay
            PacketTimer.Enabled = true;
            PacketTimer.Tick += new System.EventHandler(OnPacketTimerTick);

            RXbuffer = new byte[RX_BUF_SIZE];

            m_state_covariance = new float[3, 3];

            m_accel_alignment = new float[3, 3];
            m_gyro_alignment = new float[3, 3];
            m_mag_cal = new float[3, 3];
            m_state_covariance = new float[3, 3];
        }

        // Destructor
        ~AHRS()
        {
            // If the serial port is open, close it.
            if (connected)
            {
                serialPort.Close();
            }

            PacketTimer.Stop();
            PacketTimer.Dispose();
        }

        /* **********************************************************************************
         * 
         * Private member variables
         * 
         * *********************************************************************************/
        
        // Events for interfacing with AHRS class
        // A PacketReveivedEvent occurs when a new packet is received and parsed.
        public event PacketDelegate PacketReceivedEvent;
        public event PacketDelegate PacketSentEvent;
        public event COMFailedDelegate COMFailedEvent;
        // A DataReceivedEvent occurs when new sensor data arrives from the AHRS.
        public event DataReceivedDelegate DataReceivedEvent;
        // A COMFailedEvent occurs when the AHRS class expects to receive data from the
        // AHRS device, but no data is received in the max. allowed time.
        public event StateDelegate PacketTimeoutEvent;
        
        // Data for communication
        private bool connected;
        private SerialPort serialPort;
        private Timer PacketTimer;

        const int RX_BUF_SIZE = 500;
        public byte[] RXbuffer { get; set;  }
        private int RXbufPtr;
        const int MAX_PACKET_SIZE = 40;

        const int CHANNEL_COUNT = 15;

        const int YAW_INDEX = 0;
        const int PITCH_INDEX = 1;
        const int ROLL_INDEX = 2;
        const int YAW_RATE_INDEX = 3;
        const int PITCH_RATE_INDEX = 4;
        const int ROLL_RATE_INDEX = 5;
        const int X_MAG_INDEX = 6;
        const int Y_MAG_INDEX = 7;
        const int Z_MAG_INDEX = 8;
        const int X_ACCEL_INDEX = 9;
        const int Y_ACCEL_INDEX = 10;
        const int Z_ACCEL_INDEX = 11;
        const int X_GYRO_INDEX = 12;
        const int Y_GYRO_INDEX = 13;
        const int Z_GYRO_INDEX = 14;

        // Array for storing the most recent data obtained from the AHRS
        private Int16[] m_recentData = new Int16[CHANNEL_COUNT];

        private double m_pitchAngle;
        private double m_rollAngle;
        private double m_yawAngle;
        private double m_pitchRate;
        private double m_rollRate;
        private double m_yawRate;
        private double m_magX;
        private double m_magY;
        private double m_magZ;
        private double m_gyroX;
        private double m_gyroY;
        private double m_gyroZ;
        private double m_accelX;
        private double m_accelY;
        private double m_accelZ;

        // Variables for storing the state of the AHRS
        private float[,] m_accel_alignment;
        private float[,] m_gyro_alignment;
        private float[,] m_mag_cal;
        private float[,] m_state_covariance;
        private float m_process_covariance;
        private float m_mag_covariance;
        private float m_accel_covariance;
        private float m_x_gyro_scale;
        private float m_y_gyro_scale;
        private float m_z_gyro_scale;
        private Int16 m_x_gyro_bias;
        private Int16 m_y_gyro_bias;
        private Int16 m_z_gyro_bias;
        private Int16 m_x_accel_bias;
        private Int16 m_y_accel_bias;
        private Int16 m_z_accel_bias;
        private Int16 m_x_mag_bias;
        private Int16 m_y_mag_bias;
        private Int16 m_z_mag_bias;
        private Int16 m_x_mag_ref;
        private Int16 m_y_mag_ref;
        private Int16 m_z_mag_ref;
        private Int16 m_x_accel_ref;
        private Int16 m_y_accel_ref;
        private Int16 m_z_accel_ref;
        private bool m_mag_enabled;
        private bool m_accel_enabled;
        private bool m_mag_restriced_to_yaw;
        private bool m_broadcast_enabled;
        private UInt16 m_broadcast_rate;
        private bool m_zero_gyros_on_startup;

        private bool m_accelX_active;
        private bool m_accelY_active;
        private bool m_accelZ_active;
        private bool m_gyroX_active;
        private bool m_gyroY_active;
        private bool m_gyroZ_active;
        private bool m_magX_active;
        private bool m_magY_active;
        private bool m_magZ_active;
        private bool m_pitch_rate_active;
        private bool m_roll_rate_active;
        private bool m_yaw_rate_active;
        private bool m_pitch_active;
        private bool m_roll_active;
        private bool m_yaw_active;

        public bool self_test_completed { get; set; }
        public UInt16 self_test_results { get; set; }

        public double yawAngle
        {
            get { return m_yawAngle; }
        }

        public double pitchAngle
        {
            get { return m_pitchAngle; }
        }

        public double rollAngle
        {
            get { return m_rollAngle; }
        }

        public double yawRate
        {
            get { return m_yawRate; }
        }

        public double pitchRate
        {
            get { return m_pitchRate; }
        }

        public double rollRate
        {
            get { return m_rollRate; }
        }

        public double magX
        {
            get { return m_magX; }
        }

        public double magY
        {
            get { return m_magY; }
        }

        public double magZ
        {
            get { return m_magZ; }
        }

        public double gyroX
        {
            get { return m_gyroX; }
        }

        public double gyroY
        {
            get { return m_gyroY; }
        }

        public double gyroZ
        {
            get { return m_gyroZ; }
        }

        public double accelX
        {
            get { return m_accelX; }
        }

        public double accelY
        {
            get { return m_accelY; }
        }

        public double accelZ
        {
            get { return m_accelZ; }
        }

        public float[,] accel_alignment
        {
            get { return m_accel_alignment; }
            set { m_accel_alignment = value; UpdatePending[(int)StateName.ACCEL_ALIGNMENT] = true; }
        }

        public float[,] gyro_alignment
        {
            get { return m_gyro_alignment; }
            set { m_gyro_alignment = value; UpdatePending[(int)StateName.GYRO_ALIGNMENT] = true; }
        }

        public float[,] mag_cal
        {
            get { return m_mag_cal; }
            set { m_mag_cal = value; UpdatePending[(int)StateName.MAG_CAL] = true; }
        }

        public float[,] state_covariance
        {
            get { return m_state_covariance; }
        }

        // Public accessor methods
        public Int16[] recentData
        {
            get { return m_recentData; }
        }
        
        public bool accelX_active
        {
            get { return m_accelX_active; }
            set { m_accelX_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool accelY_active
        {
            get { return m_accelY_active; }
            set { m_accelY_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool accelZ_active
        {
            get { return m_accelZ_active; }
            set { m_accelZ_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool gyroX_active
        {
            get { return m_gyroX_active; }
            set { m_gyroX_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool gyroY_active
        {
            get { return m_gyroY_active; }
            set { m_gyroY_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool gyroZ_active
        {
            get { return m_gyroZ_active; }
            set { m_gyroZ_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool magX_active
        {
            get { return m_magX_active; }
            set { m_magX_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool magY_active
        {
            get { return m_magY_active; }
            set { m_magY_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool magZ_active
        {
            get { return m_magZ_active; }
            set { m_magZ_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool pitch_active
        {
            get { return m_pitch_active; }
            set { m_pitch_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool roll_active
        {
            get { return m_roll_active; }
            set { m_roll_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool yaw_active
        {
            get { return m_yaw_active; }
            set { m_yaw_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool pitch_rate_active
        {
            get { return m_pitch_rate_active; }
            set { m_pitch_rate_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool roll_rate_active
        {
            get { return m_roll_rate_active; }
            set { m_roll_rate_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }

        public bool yaw_rate_active
        {
            get { return m_yaw_rate_active; }
            set { m_yaw_rate_active = value; UpdatePending[(int)StateName.ACTIVE_CHANNELS] = true; }
        }
        
        public float process_covariance
        {
            get { return m_process_covariance; }
            set { m_process_covariance = value; UpdatePending[(int)StateName.PROCESS_COVARIANCE] = true; }
        }

        public float mag_covariance
        {
            get { return m_mag_covariance; }
            set { m_mag_covariance = value; UpdatePending[(int)StateName.MAG_COVARIANCE] = true; }
        }

        public float accel_covariance
        {
            get { return m_accel_covariance; }
            set { m_accel_covariance = value; UpdatePending[(int)StateName.ACCEL_COVARIANCE] = true; }
        }

        public float x_gyro_scale
        {
            get { return m_x_gyro_scale; }
            set { m_x_gyro_scale = value; UpdatePending[(int)StateName.GYRO_SCALE] = true; }
        }

        public float y_gyro_scale
        {
            get { return m_y_gyro_scale; }
            set { m_y_gyro_scale = value; UpdatePending[(int)StateName.GYRO_SCALE] = true; }
        }

        public float z_gyro_scale
        {
            get { return m_z_gyro_scale; }
            set { m_z_gyro_scale = value; UpdatePending[(int)StateName.GYRO_SCALE] = true; }
        }

        public Int16 x_gyro_bias
        {
            get { return m_x_gyro_bias; }
            set { m_x_gyro_bias = value; UpdatePending[(int)StateName.GYRO_BIAS] = true; }
        }

        public Int16 y_gyro_bias
        {
            get { return m_y_gyro_bias; }
            set { m_y_gyro_bias = value; UpdatePending[(int)StateName.GYRO_BIAS] = true; }
        }

        public Int16 z_gyro_bias
        {
            get { return m_z_gyro_bias; }
            set { m_z_gyro_bias = value; UpdatePending[(int)StateName.GYRO_BIAS] = true; }
        }

        public Int16 x_accel_bias
        {
            get { return m_x_accel_bias; }
            set { m_x_accel_bias = value; UpdatePending[(int)StateName.ACCEL_BIAS] = true; }
        }

        public Int16 y_accel_bias
        {
            get { return m_y_accel_bias; }
            set { m_y_accel_bias = value; UpdatePending[(int)StateName.ACCEL_BIAS] = true; }
        }

        public Int16 z_accel_bias
        {
            get { return m_z_accel_bias; }
            set { m_z_accel_bias = value; UpdatePending[(int)StateName.ACCEL_BIAS] = true; }
        }

        public Int16 x_mag_bias
        {
            get { return m_x_mag_bias; }
            set { m_x_mag_bias = value; UpdatePending[(int)StateName.MAG_BIAS] = true; }
        }

        public Int16 y_mag_bias
        {
            get { return m_y_mag_bias; }
            set { m_y_mag_bias = value; UpdatePending[(int)StateName.MAG_BIAS] = true; }
        }

        public Int16 z_mag_bias
        {
            get { return m_z_mag_bias; }
            set { m_z_mag_bias = value; UpdatePending[(int)StateName.MAG_BIAS] = true; }
        }

        public Int16 x_mag_ref
        {
            get { return m_x_mag_ref; }
            set { m_x_mag_ref = value; UpdatePending[(int)StateName.MAG_REF] = true; }
        }

        public Int16 y_mag_ref
        {
            get { return m_y_mag_ref; }
            set { m_y_mag_ref = value; UpdatePending[(int)StateName.MAG_REF] = true; }
        }

        public Int16 z_mag_ref
        {
            get { return m_z_mag_ref; }
            set { m_z_mag_ref = value; UpdatePending[(int)StateName.MAG_REF] = true; }
        }

        public Int16 x_accel_ref
        {
            get { return m_x_accel_ref; }
            set { m_x_accel_ref = value; UpdatePending[(int)StateName.ACCEL_REF] = true; }
        }

        public Int16 y_accel_ref
        {
            get { return m_y_accel_ref; }
            set { m_y_accel_ref = value; UpdatePending[(int)StateName.ACCEL_REF] = true; }
        }

        public Int16 z_accel_ref
        {
            get { return m_z_accel_ref; }
            set { m_z_accel_ref = value; UpdatePending[(int)StateName.ACCEL_REF] = true; }
        }

        public bool mag_enabled
        {
            get { return m_mag_enabled; }
            set { m_mag_enabled = value; UpdatePending[(int)StateName.EKF_CONFIG] = true; }
        }

        public bool mag_restricted_to_yaw
        {
            get { return m_mag_restriced_to_yaw; }
            set { m_mag_restriced_to_yaw = value; UpdatePending[(int)StateName.EKF_CONFIG] = true; }
        }

        public bool accel_enabled
        {
            get { return m_accel_enabled; }
            set { m_accel_enabled = value; UpdatePending[(int)StateName.EKF_CONFIG] = true; }
        }

        public bool broadcast_enabled
        {
            get { return m_broadcast_enabled; }
            set { m_broadcast_enabled = value; UpdatePending[(int)StateName.BROADCAST_MODE] = true; }
        }

        public UInt16 broadcast_rate
        {
            get { return m_broadcast_rate; }
            set { m_broadcast_rate = value; UpdatePending[(int)StateName.BROADCAST_MODE] = true; }
        }

        public bool zero_gyros_on_startup
        {
            get { return m_zero_gyros_on_startup; }
            set { m_zero_gyros_on_startup = value; UpdatePending[(int)StateName.START_CAL] = true; }
        }

        public bool IsConnected
        {
            get { return connected; }
        }
        
        // Arrays for definition of the AHRS COM protocol.  The enum PName references the names
        // of all possible packets that can be transmitted and received by the AHRS class.
        // The PID array stores the packet IDs associated with each packet name in PName.
        Byte[] PID;
        // The 'DataPending' flag is 'true' when a packet has been sent to the AHRS and the
        // AHRS class is awaiting a response.  This applies for both data requests and for
        // commands.
        bool[] DataPending;
        // For each packet sent, there is a MaxDelay in milliseconds that the AHRS will wait
        // to receive a response from the AHRS before assuming that the AHRS did not receive
        // the packet.  The 'ElapsedTime' array is used to store the amount of time that has
        // passed since the initiating packet was sent.
        int[] MaxDelay;
        int[] ElapsedTime;

        // The 'UpdatePending' flah is 'true' when the internal class state has been changed,
        // but the changes have not been transmitted to the AHRS itself.
        bool[] UpdatePending;

        // The 'Measured' flag is true when data has been received from AHRS - this applies to
        // AHRS configuration states that are stored internally by the AHRS class.
        bool[] Measured;


        /* **********************************************************************************
         * 
         * Function: private UInt16 Invalidate()
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS class to assume that none of its internal data is correct.
         * After Invalidate has been called, a call to 'synch' will cause the class
         * to re-acquire all data from the AHRS device.
         * 
         * *********************************************************************************/
        public void Invalidate()
        {
            // Clear 'update' and 'measured' data members.
            for (int i = 0; i < UpdatePending.Length; i++)
            {
                UpdatePending[i] = false;
                Measured[i] = false;

                // Set maximum delay in milliseconds.  Set to 200 ms for each packet - special cases (ie. packets that take longer) are
                // set later.
                MaxDelay[i] = 1000;
                ElapsedTime[i] = 0;
                DataPending[i] = false;
            }

            // Allow self-test 2000 ms to complete
            MaxDelay[(int)StateName.SELF_TEST] = 2000;

            // Allow zero_rate_gyros 2000 ms to complete
            MaxDelay[(int)StateName.ZERO_GYROS] = 2000;

            // Allow 2000 ms for reset_to_factory command
            MaxDelay[(int)StateName.RESET_TO_FACTORY] = 2000;

            // Allow 2000 ms for WRITE_TO_FLASH command to complete
            MaxDelay[(int)StateName.WRITE_TO_FLASH] = 2000;

            // Set 'Measured' flag to 'true' for packets that should not be sent on synchronization.
            Measured[(int)StateName.WRITE_TO_FLASH] = true;
            Measured[(int)StateName.ZERO_GYROS] = true;
            Measured[(int)StateName.SELF_TEST] = true;
            Measured[(int)StateName.RESET_TO_FACTORY] = true;
            Measured[(int)StateName.EKF_RESET] = true;
            Measured[(int)StateName.SENSOR_DATA] = true;
            Measured[(int)StateName.STATE_COVARIANCE] = true;
            Measured[(int)StateName.AUTO_SET_ACCEL_REF] = true;
            Measured[(int)StateName.AUTO_SET_MAG_REF] = true;
        }

        /* **********************************************************************************
         * 
         * Function: public static void OnPacketTimerTick
         * Inputs: object source, EventArgs e
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * On each timer "tick," the DataPending array is checked to determine which packets
         * have been sent and are awaiting a response.  For each pending response, the amount
         * of time that has elapsed since the packet was sent is checked.  If more time has
         * elapsed than the time (in ms) specified by MaxDelay[], then assume the operation
         * has failed.  Trigger a COMFailed event for the packet.
         * 
         * *********************************************************************************/
        public void OnPacketTimerTick(object source, EventArgs e)
        {
            int i;

            for (i = 0; i < ElapsedTime.Length; i++)
            {
                if (DataPending[i])
                {
                    ElapsedTime[i] += PacketTimer.Interval;
                    if (ElapsedTime[i] >= MaxDelay[i])
                    {
                        // Set DataPending flag to false, reset elapsed time.
                        DataPending[i] = false;
                        ElapsedTime[i] = 0;

                        // Trigger a COMFailed event
                        PacketTimeoutEvent((StateName)i, 0);
                    }
                }
            }
        }

        /* **********************************************************************************
         * 
         * Function: public bool connect( String portname )
         * Inputs: None
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Connects to the specified serial port.  Returns true on success, false on failure
         * 
         * *********************************************************************************/
        public bool connect(String portname)
        {
            //set the properties of the SerialPort Object
            serialPort.PortName = portname;
            serialPort.BaudRate = 115200;
            serialPort.Parity = Parity.None;
            serialPort.DataBits = 8;
            serialPort.StopBits = StopBits.One;
            serialPort.Handshake = Handshake.None;

            try
            {
                //now open the port
                serialPort.Open();

                connected = true;

                // Add event handler
                serialPort.DataReceived += new SerialDataReceivedEventHandler(serialPort_DataReceived);

                return true;
            }
            catch
            {
                return false;
            }
        }

        /* **********************************************************************************
         * 
         * Function: public bool Disconnect( )
         * Inputs: None
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Disconnects from the serial port
         * 
         * *********************************************************************************/
        public bool Disconnect()
        {
            if (connected)
            {
                serialPort.Close();
                connected = false;
            }

            return true;
        }

        /* **********************************************************************************
         * 
         * Function: private void serialPort_DataReceived
         * Inputs: object sender, SerialDataReceivedEventArgs e
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Event handler for serial communication
         * 
         * *********************************************************************************/
        private void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            int continue_parsing = 1;
            int bytes_to_read;

            try
            {
                bytes_to_read = serialPort.BytesToRead;

                if ((RXbufPtr + bytes_to_read) >= RX_BUF_SIZE)
                {
                    RXbufPtr = 0;
                }

                if (bytes_to_read >= RX_BUF_SIZE)
                {
                    bytes_to_read = RX_BUF_SIZE - 1;
                }
            
                // Get serial data
                serialPort.Read(RXbuffer, RXbufPtr, bytes_to_read);

                RXbufPtr += bytes_to_read;
            }
            catch
            {
                COMFailedEvent();
                return;
            }

            bool found_packet;
            int packet_start_index;
            int packet_index;

            // If there are enough bytes in the buffer to construct a full packet, then check data.
            // There are RXbufPtr bytes in the buffer at any given time
            while (RXbufPtr >= 7 && (continue_parsing == 1))
            {
                // Search for the packet start sequence
                found_packet = false;
                packet_start_index = 0;
                for (packet_index = 0; packet_index < (RXbufPtr - 2); packet_index++)
                {
                    if (RXbuffer[packet_index] == 's' && RXbuffer[packet_index + 1] == 'n' && RXbuffer[packet_index + 2] == 'p')
                    {
                        found_packet = true;
                        packet_start_index = packet_index;

                        break;
                    }
                }

                // If start sequence found, try to recover all the data in the packet
                if (found_packet && ((RXbufPtr - packet_start_index) >= 7))
                {
                    byte packet_type = RXbuffer[packet_start_index + 3];
                    byte data_size = RXbuffer[packet_start_index + 4];
                    byte[] data = new byte[MAX_PACKET_SIZE];

                    // Only process packet if data_size is not too large.
                    if (data_size <= MAX_PACKET_SIZE)
                    {

                        // If a full packet has been received, then the full packet size should be
                        // 3 + 1 + 1 + [data_size] + 2
                        // that is, 3 bytes for the start sequence, 1 byte for type, 1 byte for data length, data_size bytes
                        // for packet data, and 2 bytes for the checksum.
                        // If enough data has been received, go ahead and recover the packet.  If not, wait until the
                        // rest of the data arrives
                        int buffer_length = (RXbufPtr - packet_start_index);
                        int packet_length = (7 + data_size);
                        if (buffer_length >= packet_length)
                        {
                            // A full packet has been received.  Retrieve the data.
                            for (int i = 0; i < data_size; i++)
                            {
                                data[i] = RXbuffer[packet_start_index + 5 + i];
                            }

                            handle_packet(packet_type, (int)data_size, data);

                            // Copy all received bytes that weren't part of this packet into the beginning of the
                            // buffer.  Then, reset RXbufPtr.
                            for (int index = 0; index < (buffer_length - packet_length); index++)
                            {
                                RXbuffer[index] = RXbuffer[(packet_start_index + packet_length) + index];
                            }

                            RXbufPtr = (buffer_length - packet_length);
                        }
                        else
                        {
                            continue_parsing = 0;
                        }
                    }
                    else
                    {
                        // data_size was too large - the packet data is invalid.  Clear the RX buffer.
                        RXbufPtr = 0;
                        continue_parsing = 0;
//                        AppendStatusText("\r\nBAD PACKET", Color.Red);
                    }
                }
                else
                {
                    continue_parsing = 0;
                }
            }
        }

        /* **********************************************************************************
         * 
         * Function: private int getTypeIndex
         * Inputs: byte type
         * Outputs: None
         * Return Value: The index of the PID in 'byte type'
         * Dependencies: None
         * Description: 
         * 
         * Finds the index of the packet type of the packet specified by 'type'.  If no
         * packet was found, returns -1.
         * 
         * *********************************************************************************/
        private int getTypeIndex(byte type)
        {
            int type_index = -1;

            // Iterate through the PID array to determine which packet was received
            // (ie. which enum PName was received).
            for (int i = 0; i < PID.Length; i++)
            {
                if (PID[i] == type)
                {
                    type_index = i;
                    break;
                }
            }

            return type_index;
        }

        /* **********************************************************************************
         * 
         * Function: private void updatePacketSynch
         * Inputs: PName packet
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * The AHRS class keeps track of packets it has sent and received - if a packet requesting
         * data is sent to the AHRS device, then the class will watch to receive the data.
         * If the data is not received in MaxDelay milliseconds, then a PacketTimeoutEvent is
         * fired (from elsewhere in the class, not from the updatePacketSynch function).
         * 
         * Timing is tracked using several arrays:
         * The ElapsedTime[] array keeps track of how much time has elapsed since a packet
         * requesting data was sent.
         * The DataPending[] array keeps track of which channels have "pending" data - ie.
         * which channels the AHRS class is expecting to receive data on.
         * The Measured[] array keeps track of which data has been received at least once
         * from the AHRS - this allows the AHRS class to synchronize itself with the device.
         * The UpdatePending[] arrays keeps track of which internal class data has been changed,
         * but has not yet been written to the AHRS.
         * 
         * Whenever a new packet is received from the AHRS device, the DataPending array
         * needs to be updated to reflect the data that was just received - if data was received,
         * then it is no longer "pending."
         * 
         * When a packet is received, the Measured[] array should also be updated - we've
         * received data from the channel.  The ElapsedTime[] array for the received data should
         * also be set to 0.
         * 
         * The updatePacketSynch function performs the aforementioned tasks.
         * 
         * *********************************************************************************/
        private void updatePacketSynch(PName packet, byte[] data)
        {
            switch (packet)
            {
                case PName.ACCEL_ALIGNMENT_REPORT:
                    updatePacketSynchHelper(StateName.ACCEL_ALIGNMENT);
                    break;

                case PName.ACCEL_COVARIANCE_REPORT:
                    updatePacketSynchHelper(StateName.ACCEL_COVARIANCE);
                    break;

                case PName.ACCEL_BIAS_REPORT:
                    updatePacketSynchHelper(StateName.ACCEL_BIAS);
                    break;

                case PName.ACCEL_REF_VECTOR_REPORT:
                    updatePacketSynchHelper(StateName.ACCEL_REF);
                    updatePacketSynchHelper(StateName.AUTO_SET_ACCEL_REF);
                    break;

                case PName.ACTIVE_CHANNEL_REPORT:
                    updatePacketSynchHelper(StateName.ACTIVE_CHANNELS);
                    break;

                case PName.BROADCAST_MODE_REPORT:
                    updatePacketSynchHelper(StateName.BROADCAST_MODE);
                    break;

                case PName.COMMAND_COMPLETE:
                    int type_index = getTypeIndex(data[0]);
                
                    switch((PName)type_index)
                    {
                        case PName.EKF_RESET:
                            updatePacketSynchHelper(StateName.EKF_RESET);
                            break;

                        case PName.RESET_TO_FACTORY:
                            updatePacketSynchHelper(StateName.RESET_TO_FACTORY);
                            break;

                        case PName.WRITE_TO_FLASH:
                            updatePacketSynchHelper(StateName.WRITE_TO_FLASH);
                            break;

                        case PName.SET_ACCEL_ALIGNMENT:
                            updatePacketSynchHelper(StateName.ACCEL_ALIGNMENT);
                            break;

                        case PName.SET_ACCEL_COVARIANCE:
                            updatePacketSynchHelper(StateName.ACCEL_COVARIANCE);
                            break;

                        case PName.SET_ACCEL_BIAS:
                            updatePacketSynchHelper(StateName.ACCEL_BIAS);
                            break;

                        case PName.SET_ACCEL_REF_VECTOR:
                            updatePacketSynchHelper(StateName.ACCEL_REF);
                            break;

                        case PName.SET_ACTIVE_CHANNELS:
                            updatePacketSynchHelper(StateName.ACTIVE_CHANNELS);
                            break;

                        case PName.SET_BROADCAST_MODE:
                            updatePacketSynchHelper(StateName.BROADCAST_MODE);
                            break;

                        case PName.SET_EKF_CONFIG:
                            updatePacketSynchHelper(StateName.EKF_CONFIG);
                            break;

                        case PName.SET_GYRO_ALIGNMENT:
                            updatePacketSynchHelper(StateName.GYRO_ALIGNMENT);
                            break;

                        case PName.SET_GYRO_BIAS:
                            updatePacketSynchHelper(StateName.GYRO_BIAS);
                            break;

                        case PName.SET_GYRO_SCALE:
                            updatePacketSynchHelper(StateName.GYRO_SCALE);
                            break;

                        case PName.SET_MAG_CAL:
                            updatePacketSynchHelper(StateName.MAG_CAL);
                            break;

                        case PName.SET_MAG_BIAS:
                            updatePacketSynchHelper(StateName.MAG_BIAS);
                            break;

                        case PName.SET_MAG_REF_VECTOR:
                            updatePacketSynchHelper(StateName.MAG_REF);
                            break;

                        case PName.SET_MAG_COVARIANCE:
                            updatePacketSynchHelper(StateName.MAG_COVARIANCE);
                            break;

                        case PName.SET_PROCESS_COVARIANCE:
                            updatePacketSynchHelper(StateName.PROCESS_COVARIANCE);
                            break;

                        case PName.SET_SILENT_MODE:
                            updatePacketSynchHelper(StateName.BROADCAST_MODE);
                            break;

                        case PName.SET_START_CAL:
                            updatePacketSynchHelper(StateName.START_CAL);
                            break;

                        default:
                            break;
                    }

                    break;

                case PName.EKF_CONFIG_REPORT:
                    updatePacketSynchHelper(StateName.EKF_CONFIG);
                    break;

                case PName.GYRO_ALIGNMENT_REPORT:
                    updatePacketSynchHelper(StateName.GYRO_ALIGNMENT);
                    break;

                case PName.GYRO_BIAS_REPORT:
                    updatePacketSynchHelper(StateName.GYRO_BIAS);
                    updatePacketSynchHelper(StateName.ZERO_GYROS);
                    break;

                case PName.GYRO_SCALE_REPORT:
                    updatePacketSynchHelper(StateName.GYRO_SCALE);
                    break;

                case PName.MAG_COVARIANCE_REPORT:
                    updatePacketSynchHelper(StateName.MAG_COVARIANCE);
                    break;

                case PName.MAG_BIAS_REPORT:
                    updatePacketSynchHelper(StateName.MAG_BIAS);
                    break;

                case PName.MAG_REF_VECTOR_REPORT:
                    updatePacketSynchHelper(StateName.MAG_REF);
                    updatePacketSynchHelper(StateName.AUTO_SET_MAG_REF);
                    break;

                case PName.PROCESS_COVARIANCE_REPORT:
                    updatePacketSynchHelper(StateName.PROCESS_COVARIANCE);
                    break;

                case PName.STATUS_REPORT:
                    updatePacketSynchHelper(StateName.SELF_TEST);
                    break;

                case PName.SENSOR_DATA:
                    updatePacketSynchHelper(StateName.SENSOR_DATA);
                    break;

                case PName.START_CAL_REPORT:
                    updatePacketSynchHelper(StateName.START_CAL);
                    break;

                case PName.STATE_COVARIANCE_REPORT:
                    updatePacketSynchHelper(StateName.STATE_COVARIANCE);
                    break;

                case PName.MAG_CAL_REPORT:
                    updatePacketSynchHelper(StateName.MAG_CAL);
                    break;

                default:
                    break;
            }

        }

        private void updatePacketSynchHelper(StateName state)
        {
            DataPending[(int)state] = false;
            ElapsedTime[(int)state] = 0;
            Measured[(int)state] = true;
            UpdatePending[(int)state] = false;
        }

        /* **********************************************************************************
         * 
         * Function: private void handle_packet
         * Inputs: byte type, int length, byte[] data
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Handles data packets received over the serial port.
         * 
         * *********************************************************************************/
        private void handle_packet(byte type, int length, byte[] data)
        {
            int type_index = -1;
            byte_conversion_array getFloat = new byte_conversion_array();

            type_index = getTypeIndex(type);

            // For the packet received, update 'dataPending', 'measured', and 'ElapsedTime' flags
            if (type_index != -1)
            {
                updatePacketSynch((PName)type_index, data);
            }
            else
            {
                // Generate a COMMAND_COMPLETE event with a -1 flag.  The -1 indicates that the packet
                // wasn't recognized.
                PacketReceivedEvent(PName.COMMAND_COMPLETE, -1);
                return;
            }

            switch (type_index)
            {
                // COMMAND_COMPLETE
                case (int)PName.COMMAND_COMPLETE:

                    type_index = getTypeIndex(data[0]);

                    PacketReceivedEvent(PName.COMMAND_COMPLETE, type_index);

                    break;

                // COMMAND_FAILED
                case (int)PName.COMMAND_FAILED:

                    PacketReceivedEvent(PName.COMMAND_FAILED, data[0]);

                    break;

                case (int)PName.BAD_CHECKSUM:

                    PacketReceivedEvent(PName.BAD_CHECKSUM, 0);

                    break;

                case (int)PName.BAD_DATA_LENGTH:

                    PacketReceivedEvent(PName.BAD_DATA_LENGTH, 0);

                    break;

                case (int)PName.UNRECOGNIZED_PACKET:

                    PacketReceivedEvent(PName.UNRECOGNIZED_PACKET, 0);

                    break;

                case (int)PName.BUFFER_OVERFLOW:

                    PacketReceivedEvent(PName.BUFFER_OVERFLOW, 0);

                    break;

                // Status Packet
                case (int)PName.STATUS_REPORT:

                    self_test_completed = true;
                    self_test_results = data[0];

                    PacketReceivedEvent(PName.STATUS_REPORT, data[0]);

                    break;

                // SENSOR_DATA packet
                case (int)PName.SENSOR_DATA:

                    int act_channels = (data[0] << 8) | (data[1]);
                    int i = 2;

                    // Yaw angle
                    if ((act_channels & 0x8000) != 0)
                    {
                        m_recentData[YAW_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_yawAngle = (float)m_recentData[YAW_INDEX] * (.0109863);
                        i += 2;
                    }

                    // Pitch angle
                    if ((act_channels & 0x4000) != 0)
                    {
                        m_recentData[PITCH_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_pitchAngle = (float)m_recentData[PITCH_INDEX] * (.0109863);
                        i += 2;
                    }

                    // Roll angle
                    if ((act_channels & 0x2000) != 0)
                    {
                        m_recentData[ROLL_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_rollAngle = (float)m_recentData[ROLL_INDEX] * (.0109863);
                        i += 2;
                    }

                    // Yaw rate
                    if ((act_channels & 0x1000) != 0)
                    {
                        m_recentData[YAW_RATE_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_yawRate = (float)m_recentData[YAW_RATE_INDEX] * (.0137329);
                        i += 2;
                    }

                    // Pitch rate
                    if ((act_channels & 0x0800) != 0)
                    {
                        m_recentData[PITCH_RATE_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_pitchRate = (float)m_recentData[PITCH_RATE_INDEX] * (.0137329);
                        i += 2;
                    }

                    // Roll rate
                    if ((act_channels & 0x0400) != 0)
                    {
                        m_recentData[ROLL_RATE_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_rollRate = (float)m_recentData[ROLL_RATE_INDEX] * (.0137329);
                        i += 2;
                    }

                    // Mag X
                    if ((act_channels & 0x0200) != 0)
                    {
                        m_recentData[X_MAG_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_magX = (float)m_recentData[X_MAG_INDEX];
                        i += 2;
                    }

                    // Mag Y
                    if ((act_channels & 0x0100) != 0)
                    {
                        m_recentData[Y_MAG_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_magY = (float)m_recentData[Y_MAG_INDEX];
                        i += 2;
                    }

                    // Mag Z
                    if ((act_channels & 0x0080) != 0)
                    {
                        m_recentData[Z_MAG_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_magZ = (float)m_recentData[Z_MAG_INDEX];
                        i += 2;
                    }

                    // Gyro X
                    if ((act_channels & 0x0040) != 0)
                    {
                        m_recentData[X_GYRO_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_gyroX = (float)m_recentData[X_GYRO_INDEX]*m_x_gyro_scale;
                        i += 2;
                    }

                    // Gyro Y
                    if ((act_channels & 0x0020) != 0)
                    {
                        m_recentData[Y_GYRO_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_gyroY = (float)m_recentData[Y_GYRO_INDEX]*m_y_gyro_scale;
                        i += 2;
                    }

                    // Gyro Z
                    if ((act_channels & 0x0010) != 0)
                    {
                        m_recentData[Z_GYRO_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_gyroZ = (float)m_recentData[Z_GYRO_INDEX]*m_z_gyro_scale;
                        i += 2;
                    }

                    // Accel X
                    if ((act_channels & 0x0008) != 0)
                    {
                        m_recentData[X_ACCEL_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_accelX = (float)m_recentData[X_ACCEL_INDEX];
                        i += 2;
                    }

                    // Accel Y
                    if ((act_channels & 0x0004) != 0)
                    {
                        m_recentData[Y_ACCEL_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_accelY = (float)m_recentData[Y_ACCEL_INDEX];
                        i += 2;
                    }

                    // Accel Z
                    if ((act_channels & 0x0002) != 0)
                    {
                        m_recentData[Z_ACCEL_INDEX] = (Int16)((data[i] << 8) | data[i + 1]);
                        m_accelZ = (float)m_recentData[Z_ACCEL_INDEX];
                    }

                    DataReceivedEvent(act_channels);

                    break;

                // GYRO_BIAS_REPORT packet
                case (int)PName.GYRO_BIAS_REPORT:
                    m_x_gyro_bias = (Int16)((data[4] << 8) | data[5]);
                    m_y_gyro_bias = (Int16)((data[2] << 8) | data[3]);
                    m_z_gyro_bias = (Int16)((data[0] << 8) | data[1]);

                    PacketReceivedEvent(PName.GYRO_BIAS_REPORT, 0);

                    break;

                // ACCEL_SCALE_REPORT packet
                case (int)PName.ACCEL_BIAS_REPORT:

                    m_x_accel_bias = (Int16)((data[4] << 8) | data[5]);
                    m_y_accel_bias = (Int16)((data[2] << 8) | data[3]);
                    m_z_accel_bias = (Int16)((data[0] << 8) | data[1]);                    

                    PacketReceivedEvent(PName.ACCEL_BIAS_REPORT, 0);

                    break;

                // ACTIVE_CHANNEL_REPORT packet
                case (int)PName.ACTIVE_CHANNEL_REPORT:

                    int active_channels = (UInt16)(data[1] | (UInt16)(data[0] << 8));

                    if (((active_channels >> 1) & 0x01) == 0x01)
                        m_accelZ_active = true;
                    else
                        m_accelZ_active = false;
                    if (((active_channels >> 2) & 0x01) == 0x01)
                        m_accelY_active = true;
                    else
                        m_accelY_active = false;
                    if (((active_channels >> 3) & 0x01) == 0x01)
                        m_accelX_active = true;
                    else
                        m_accelX_active = false;

                    if (((active_channels >> 4) & 0x01) == 0x01)
                        m_gyroZ_active = true;
                    else
                        m_gyroZ_active = false;
                    if (((active_channels >> 5) & 0x01) == 0x01)
                        m_gyroY_active = true;
                    else
                        m_gyroY_active = false;
                    if (((active_channels >> 6) & 0x01) == 0x01)
                        m_gyroX_active = true;
                    else
                        m_gyroX_active = false;

                    if (((active_channels >> 7) & 0x01) == 0x01)
                        m_magZ_active = true;
                    else
                        m_magZ_active = false;
                    if (((active_channels >> 8) & 0x01) == 0x01)
                        m_magY_active = true;
                    else
                        m_magY_active = false;
                    if (((active_channels >> 9) & 0x01) == 0x01)
                        m_magX_active = true;
                    else
                        m_magX_active = false;

                    if (((active_channels >> 10) & 0x01) == 0x01)
                        m_roll_rate_active = true;
                    else
                        m_roll_rate_active = false;
                    if (((active_channels >> 11) & 0x01) == 0x01)
                        m_pitch_rate_active = true;
                    else
                        m_pitch_rate_active = false;
                    if (((active_channels >> 12) & 0x01) == 0x01)
                        m_yaw_rate_active = true;
                    else
                        m_yaw_rate_active = false;

                    if (((active_channels >> 13) & 0x01) == 0x01)
                        m_roll_active = true;
                    else
                        m_roll_active = false;
                    if (((active_channels >> 14) & 0x01) == 0x01)
                        m_pitch_active = true;
                    else
                        m_pitch_active = false;
                    if (((active_channels >> 15) & 0x01) == 0x01)
                        m_yaw_active = true;
                    else
                        m_yaw_active = false;

                    PacketReceivedEvent(PName.ACTIVE_CHANNEL_REPORT, 0);

                    break;

                // ACCEL_COVARIANCE_REPORT packet
                case (int)PName.ACCEL_COVARIANCE_REPORT:

                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_accel_covariance = getFloat.float0;                    

                    PacketReceivedEvent(PName.ACCEL_COVARIANCE_REPORT, 0);

                    break;

                // MAG_COVARIANCE_REPORT packet
                case (int)PName.MAG_COVARIANCE_REPORT:

                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_mag_covariance = getFloat.float0;

                    PacketReceivedEvent(PName.MAG_COVARIANCE_REPORT, 0);
                    
                    break;

                case (int)PName.MAG_BIAS_REPORT:

                    m_x_mag_bias = (Int16)((data[4] << 8) | data[5]);
                    m_y_mag_bias = (Int16)((data[2] << 8) | data[3]);
                    m_z_mag_bias = (Int16)((data[0] << 8) | data[1]);

                    PacketReceivedEvent(PName.MAG_BIAS_REPORT, 0);

                    break;

                case (int)PName.MAG_REF_VECTOR_REPORT:

                    m_x_mag_ref = (Int16)((data[4] << 8) | data[5]);
                    m_y_mag_ref = (Int16)((data[2] << 8) | data[3]);
                    m_z_mag_ref = (Int16)((data[0] << 8) | data[1]);

                    PacketReceivedEvent(PName.MAG_REF_VECTOR_REPORT, 0);

                    break;

                case (int)PName.ACCEL_REF_VECTOR_REPORT:

                    m_x_accel_ref = (Int16)((data[4] << 8) | data[5]);
                    m_y_accel_ref = (Int16)((data[2] << 8) | data[3]);
                    m_z_accel_ref = (Int16)((data[0] << 8) | data[1]);

                    PacketReceivedEvent(PName.ACCEL_REF_VECTOR_REPORT, 0);

                    break;

                // PROCESS_COVARIANCE_REPORT packet
                case (int)PName.PROCESS_COVARIANCE_REPORT:

                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_process_covariance = getFloat.float0;

                    PacketReceivedEvent(PName.PROCESS_COVARIANCE_REPORT, 0);

                    break;

                // STATE_COVARIANCE_REPORT packet
                case (int)PName.STATE_COVARIANCE_REPORT:

                    // d00
                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_state_covariance[0,0] = getFloat.float0;
                    
                    // d01
                    getFloat.byte0 = data[4];
                    getFloat.byte1 = data[5];
                    getFloat.byte2 = data[6];
                    getFloat.byte3 = data[7];
                    m_state_covariance[0,1] = getFloat.float0;

                    // d02
                    getFloat.byte0 = data[8];
                    getFloat.byte1 = data[9];
                    getFloat.byte2 = data[10];
                    getFloat.byte3 = data[11];
                    m_state_covariance[0,2] = getFloat.float0;

                    // d10
                    getFloat.byte0 = data[12];
                    getFloat.byte1 = data[13];
                    getFloat.byte2 = data[14];
                    getFloat.byte3 = data[15];
                    m_state_covariance[1,0] = getFloat.float0;

                    // d11
                    getFloat.byte0 = data[16];
                    getFloat.byte1 = data[17];
                    getFloat.byte2 = data[18];
                    getFloat.byte3 = data[19];
                    m_state_covariance[1,1] = getFloat.float0;

                    // d12
                    getFloat.byte0 = data[20];
                    getFloat.byte1 = data[21];
                    getFloat.byte2 = data[22];
                    getFloat.byte3 = data[23];
                    m_state_covariance[1,2] = getFloat.float0;

                    // d20
                    getFloat.byte0 = data[24];
                    getFloat.byte1 = data[25];
                    getFloat.byte2 = data[26];
                    getFloat.byte3 = data[27];
                    m_state_covariance[2,0] = getFloat.float0;

                    // d21
                    getFloat.byte0 = data[28];
                    getFloat.byte1 = data[29];
                    getFloat.byte2 = data[30];
                    getFloat.byte3 = data[31];
                    m_state_covariance[2,1] = getFloat.float0;

                    // d22
                    getFloat.byte0 = data[32];
                    getFloat.byte1 = data[33];
                    getFloat.byte2 = data[34];
                    getFloat.byte3 = data[35];
                    m_state_covariance[2,2] = getFloat.float0;

                    PacketReceivedEvent(PName.STATE_COVARIANCE_REPORT, 0);

                    break;

                // MAG_CAL_REPORT packet
                case (int)PName.MAG_CAL_REPORT:

                    // d00
                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_mag_cal[0, 0] = getFloat.float0;

                    // d01
                    getFloat.byte0 = data[4];
                    getFloat.byte1 = data[5];
                    getFloat.byte2 = data[6];
                    getFloat.byte3 = data[7];
                    m_mag_cal[0, 1] = getFloat.float0;

                    // d02
                    getFloat.byte0 = data[8];
                    getFloat.byte1 = data[9];
                    getFloat.byte2 = data[10];
                    getFloat.byte3 = data[11];
                    m_mag_cal[0, 2] = getFloat.float0;

                    // d10
                    getFloat.byte0 = data[12];
                    getFloat.byte1 = data[13];
                    getFloat.byte2 = data[14];
                    getFloat.byte3 = data[15];
                    m_mag_cal[1, 0] = getFloat.float0;

                    // d11
                    getFloat.byte0 = data[16];
                    getFloat.byte1 = data[17];
                    getFloat.byte2 = data[18];
                    getFloat.byte3 = data[19];
                    m_mag_cal[1, 1] = getFloat.float0;

                    // d12
                    getFloat.byte0 = data[20];
                    getFloat.byte1 = data[21];
                    getFloat.byte2 = data[22];
                    getFloat.byte3 = data[23];
                    m_mag_cal[1, 2] = getFloat.float0;

                    // d20
                    getFloat.byte0 = data[24];
                    getFloat.byte1 = data[25];
                    getFloat.byte2 = data[26];
                    getFloat.byte3 = data[27];
                    m_mag_cal[2, 0] = getFloat.float0;

                    // d21
                    getFloat.byte0 = data[28];
                    getFloat.byte1 = data[29];
                    getFloat.byte2 = data[30];
                    getFloat.byte3 = data[31];
                    m_mag_cal[2, 1] = getFloat.float0;

                    // d22
                    getFloat.byte0 = data[32];
                    getFloat.byte1 = data[33];
                    getFloat.byte2 = data[34];
                    getFloat.byte3 = data[35];
                    m_mag_cal[2, 2] = getFloat.float0;

                    PacketReceivedEvent(PName.MAG_CAL_REPORT, 0);

                    break;

                // EKF_CONFIG_REPORT packet
                case (int)PName.EKF_CONFIG_REPORT:
                    // Determine whether magnetometer updates are enabled
                    if (((int)data[0] & 0x01) == 0x01)
                    {
                        m_mag_enabled = true;
                    }
                    else
                    {
                        m_mag_enabled = false;
                    }

                    // Determine whether accel updates are enabled
                    if (((int)data[0] & 0x02) == 0x02)
                    {
                        m_accel_enabled = true;
                    }
                    else
                    {
                        m_accel_enabled = false;
                    }

                    // Determine whether MAG updates are restricted to yaw only
                    if (((int)data[0] & 0x04) == 0x04)
                    {
                        m_mag_restriced_to_yaw = true;
                    }
                    else
                    {
                        m_mag_restriced_to_yaw = false;
                    }

                    PacketReceivedEvent(PName.EKF_CONFIG_REPORT, 0);

                    break;

                // GYRO_ALIGNMENT_REPORT packet
                case (int)PName.GYRO_ALIGNMENT_REPORT:

                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_gyro_alignment[0,0] = getFloat.float0;

                    getFloat.byte0 = data[4];
                    getFloat.byte1 = data[5];
                    getFloat.byte2 = data[6];
                    getFloat.byte3 = data[7];
                    m_gyro_alignment[0,1] = getFloat.float0;

                    getFloat.byte0 = data[8];
                    getFloat.byte1 = data[9];
                    getFloat.byte2 = data[10];
                    getFloat.byte3 = data[11];
                    m_gyro_alignment[0,2] = getFloat.float0;

                    getFloat.byte0 = data[12];
                    getFloat.byte1 = data[13];
                    getFloat.byte2 = data[14];
                    getFloat.byte3 = data[15];
                    m_gyro_alignment[1, 0] = getFloat.float0;

                    getFloat.byte0 = data[16];
                    getFloat.byte1 = data[17];
                    getFloat.byte2 = data[18];
                    getFloat.byte3 = data[19];
                    m_gyro_alignment[1, 1] = getFloat.float0;

                    getFloat.byte0 = data[20];
                    getFloat.byte1 = data[21];
                    getFloat.byte2 = data[22];
                    getFloat.byte3 = data[23];
                    m_gyro_alignment[1, 2] = getFloat.float0;

                    getFloat.byte0 = data[24];
                    getFloat.byte1 = data[25];
                    getFloat.byte2 = data[26];
                    getFloat.byte3 = data[27];
                    m_gyro_alignment[2, 0] = getFloat.float0;

                    getFloat.byte0 = data[28];
                    getFloat.byte1 = data[29];
                    getFloat.byte2 = data[30];
                    getFloat.byte3 = data[31];
                    m_gyro_alignment[2, 1] = getFloat.float0;

                    getFloat.byte0 = data[32];
                    getFloat.byte1 = data[33];
                    getFloat.byte2 = data[34];
                    getFloat.byte3 = data[35];
                    m_gyro_alignment[2, 2] = getFloat.float0;

                    PacketReceivedEvent(PName.GYRO_ALIGNMENT_REPORT, 0);

                    break;

                // ACCEL_ALIGNMENT_REPORT
                case (int)PName.ACCEL_ALIGNMENT_REPORT:

                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_accel_alignment[0, 0] = getFloat.float0;

                    getFloat.byte0 = data[4];
                    getFloat.byte1 = data[5];
                    getFloat.byte2 = data[6];
                    getFloat.byte3 = data[7];
                    m_accel_alignment[0, 1] = getFloat.float0;

                    getFloat.byte0 = data[8];
                    getFloat.byte1 = data[9];
                    getFloat.byte2 = data[10];
                    getFloat.byte3 = data[11];
                    m_accel_alignment[0, 2] = getFloat.float0;

                    getFloat.byte0 = data[12];
                    getFloat.byte1 = data[13];
                    getFloat.byte2 = data[14];
                    getFloat.byte3 = data[15];
                    m_accel_alignment[1, 0] = getFloat.float0;

                    getFloat.byte0 = data[16];
                    getFloat.byte1 = data[17];
                    getFloat.byte2 = data[18];
                    getFloat.byte3 = data[19];
                    m_accel_alignment[1, 1] = getFloat.float0;

                    getFloat.byte0 = data[20];
                    getFloat.byte1 = data[21];
                    getFloat.byte2 = data[22];
                    getFloat.byte3 = data[23];
                    m_accel_alignment[1, 2] = getFloat.float0;

                    getFloat.byte0 = data[24];
                    getFloat.byte1 = data[25];
                    getFloat.byte2 = data[26];
                    getFloat.byte3 = data[27];
                    m_accel_alignment[2, 0] = getFloat.float0;

                    getFloat.byte0 = data[28];
                    getFloat.byte1 = data[29];
                    getFloat.byte2 = data[30];
                    getFloat.byte3 = data[31];
                    m_accel_alignment[2, 1] = getFloat.float0;

                    getFloat.byte0 = data[32];
                    getFloat.byte1 = data[33];
                    getFloat.byte2 = data[34];
                    getFloat.byte3 = data[35];
                    m_accel_alignment[2, 2] = getFloat.float0;

                    PacketReceivedEvent(PName.ACCEL_ALIGNMENT_REPORT, 0);

                    break;

                // GYRO_SCALE_REPORT
                case (int)PName.GYRO_SCALE_REPORT:

                    getFloat.byte0 = data[0];
                    getFloat.byte1 = data[1];
                    getFloat.byte2 = data[2];
                    getFloat.byte3 = data[3];
                    m_z_gyro_scale = getFloat.float0;

                    getFloat.byte0 = data[4];
                    getFloat.byte1 = data[5];
                    getFloat.byte2 = data[6];
                    getFloat.byte3 = data[7];
                    m_y_gyro_scale = getFloat.float0;

                    getFloat.byte0 = data[8];
                    getFloat.byte1 = data[9];
                    getFloat.byte2 = data[10];
                    getFloat.byte3 = data[11];
                    m_x_gyro_scale = getFloat.float0;

                    PacketReceivedEvent(PName.GYRO_SCALE_REPORT, 0);

                    break;

                case (int)PName.START_CAL_REPORT:

                    if (((int)data[0] & 0x01) == 0x01)
                        m_zero_gyros_on_startup = true;
                    else
                        m_zero_gyros_on_startup = false;

                    PacketReceivedEvent(PName.START_CAL_REPORT, 0);

                    break;

                // BROADCAST_MODE_REPORT packet
                case (int)PName.BROADCAST_MODE_REPORT:
                    m_broadcast_rate = data[0];
                    if (((int)data[1] & 0x01) == 0x01)
                        m_broadcast_enabled = true;
                    else
                        m_broadcast_enabled = false;

                    PacketReceivedEvent(PName.BROADCAST_MODE_REPORT, 0);

                    break;

                default:
                    
                    break;

            }

        }

        /* **********************************************************************************
         * 
         * Function: public bool synch()
         * Inputs: None
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Synchronizes the stored AHRS states with actual states on  the device.  There are
         * two flags used by the AHRS class to keep track of synchronization.  For each setting,
         * the _measured flag indicates that the current AHRS state has been retrieved and stored
         * by the class.  The _updated flag indicates that the class' internal state has changed,
         * but that the AHRS itself has not yet been updated with the new data.
         * 
         * The 'synch' function first updates the AHRS by sending COM packets to set all
         * data for which the _updated flag has been set.  Then, all data that has not yet been
         * measured is requested.
         * 
         * If synchronization succeeds, the function returns 'true'.  'false' otherwise.
         * 
         * *********************************************************************************/

        public bool synch()
        {
            bool complete = true;

            if (!connected)
                return false;

            // First, iterate through the PUpdatePending array to determine which AHRS states need
            // to be updated.
            for (int i = 0; i < UpdatePending.Length; i++)
            {
                if (UpdatePending[i])
                {
                    DataPending[i] = true;

                    // Call UpdateAHRS to send packet to the AHRS to synchronize data
                    if (!updateAHRS(i))
                    {
                        complete = false;
                        DataPending[i] = false;
                        
                    }
                }
            }

            // Now, iterate through PMeasured array to determing which AHRS states have yet to be measured.
            for (int i = 0; i < Measured.Length; i++ )
            {
                if (!Measured[i])
                {
                    DataPending[i] = true;

                    if (!getAHRSState(i))
                    {
                        complete = false;
                        DataPending[i] = false;
                    }
                }
            }

            return complete;
        }

        /* **********************************************************************************
         * 
         * Function: private bool updateAHRS
         * Inputs: int index
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Causes a packet to be sent to the AHRS to update its state to conform with the internal
         * state of the AHRS class.
         * 
         * Returns 'true' on success, 'false' otherwise
         * 
         * *********************************************************************************/
        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_roll_rate_active)
                    active_channels |= (1 << 10);
                if (m_pitch_rate_active)
                    active_channels |= (1 << 11);
                if (m_yaw_rate_active)
                    active_channels |= (1 << 12);

                if (m_roll_active)
                    active_channels |= (1 << 13);
                if (m_pitch_active)
                    active_channels |= (1 << 14);
                if (m_yaw_active)
                    active_channels |= (1 << 15);

                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 = 6;
                AHRSPacket.Data = new byte[AHRSPacket.DataLength];

                AHRSPacket.Data[0] = (byte)((m_z_accel_bias >> 8) & 0x0FF);
                AHRSPacket.Data[1] = (byte)(m_z_accel_bias & 0x0FF);

                AHRSPacket.Data[2] = (byte)((m_y_accel_bias >> 8) & 0x0FF);
                AHRSPacket.Data[3] = (byte)(m_y_accel_bias & 0x0FF);

                AHRSPacket.Data[4] = (byte)((m_x_accel_bias >> 8) & 0x0FF);
                AHRSPacket.Data[5] = (byte)(m_x_accel_bias & 0x0FF);
                
                sendPacket(AHRSPacket);
            }
            else if (index == (int)StateName.ACCEL_REF)
            {
                AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_REF_VECTOR];
                AHRSPacket.DataLength = 6;
                AHRSPacket.Data = new byte[AHRSPacket.DataLength];

                AHRSPacket.Data[0] = (byte)((m_z_accel_ref >> 8) & 0x0FF);
                AHRSPacket.Data[1] = (byte)(m_z_accel_ref & 0x0FF);

                AHRSPacket.Data[2] = (byte)((m_y_accel_ref >> 8) & 0x0FF);
                AHRSPacket.Data[3] = (byte)(m_y_accel_ref & 0x0FF);

                AHRSPacket.Data[4] = (byte)((m_x_accel_ref >> 8) & 0x0FF);
                AHRSPacket.Data[5] = (byte)(m_x_accel_ref & 0x0FF);

                sendPacket(AHRSPacket);
            }
            else if (index == (int)StateName.MAG_BIAS)
            {
                AHRSPacket.PacketType = PID[(int)PName.SET_MAG_BIAS];
                AHRSPacket.DataLength = 6;
                AHRSPacket.Data = new byte[AHRSPacket.DataLength];

                AHRSPacket.Data[0] = (byte)((m_z_mag_bias >> 8) & 0x0FF);
                AHRSPacket.Data[1] = (byte)(m_z_mag_bias & 0x0FF);

                AHRSPacket.Data[2] = (byte)((m_y_mag_bias >> 8) & 0x0FF);
                AHRSPacket.Data[3] = (byte)(m_y_mag_bias & 0x0FF);

                AHRSPacket.Data[4] = (byte)((m_x_mag_bias >> 8) & 0x0FF);
                AHRSPacket.Data[5] = (byte)(m_x_mag_bias & 0x0FF);

                sendPacket(AHRSPacket);
            }
            else if (index == (int)StateName.MAG_REF)
            {
                AHRSPacket.PacketType = PID[(int)PName.SET_MAG_REF_VECTOR];
                AHRSPacket.DataLength = 6;
                AHRSPacket.Data = new byte[AHRSPacket.DataLength];

                AHRSPacket.Data[0] = (byte)((m_z_mag_ref >> 8) & 0x0FF);
                AHRSPacket.Data[1] = (byte)(m_z_mag_ref & 0x0FF);

                AHRSPacket.Data[2] = (byte)((m_y_mag_ref >> 8) & 0x0FF);
                AHRSPacket.Data[3] = (byte)(m_y_mag_ref & 0x0FF);

                AHRSPacket.Data[4] = (byte)((m_x_mag_ref >> 8) & 0x0FF);
                AHRSPacket.Data[5] = (byte)(m_x_mag_ref & 0x0FF);

                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 = 6;
                AHRSPacket.Data = new byte[AHRSPacket.DataLength];

                AHRSPacket.Data[0] = (byte)((m_z_gyro_bias >> 8) & 0x0FF);
                AHRSPacket.Data[1] = (byte)(m_z_gyro_bias & 0x0FF);

                AHRSPacket.Data[2] = (byte)((m_y_gyro_bias >> 8) & 0x0FF);
                AHRSPacket.Data[3] = (byte)(m_y_gyro_bias & 0x0FF);

                AHRSPacket.Data[4] = (byte)((m_x_gyro_bias >> 8) & 0x0FF);
                AHRSPacket.Data[5] = (byte)(m_x_gyro_bias & 0x0FF);

                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.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.WRITE_TO_FLASH)
            {
                AHRSPacket.PacketType = PID[(int)PName.WRITE_TO_FLASH];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[AHRSPacket.DataLength];

                sendPacket(AHRSPacket);
            }

            return true;
        }

        /* **********************************************************************************
         * 
         * Function: private bool getAHRSState
         * Inputs: int index
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Causes a packet to be sent to the AHRS device requesting device information.  The
         * request packet to be sent is specified by 'index'.
         * 
         * Returns 'true' on success, 'false' otherwise
         * 
         * *********************************************************************************/
        private bool getAHRSState(int index)
        {
            Packet AHRSPacket = new Packet();

            if (index == (int)StateName.ACTIVE_CHANNELS)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_ACTIVE_CHANNELS];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;                    
            }
            else if (index == (int)StateName.ACCEL_ALIGNMENT)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_ACCEL_ALIGNMENT];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.ACCEL_COVARIANCE)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_ACCEL_COVARIANCE];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.ACCEL_BIAS)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_ACCEL_BIAS];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.ACCEL_REF)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_ACCEL_REF_VECTOR];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.MAG_REF)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_MAG_REF_VECTOR];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.MAG_BIAS)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_MAG_BIAS];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.BROADCAST_MODE)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_BROADCAST_MODE];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.EKF_CONFIG)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_EKF_CONFIG];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.GYRO_ALIGNMENT)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_GYRO_ALIGNMENT];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.GYRO_BIAS)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_GYRO_BIAS];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.GYRO_SCALE)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_GYRO_SCALE];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.MAG_COVARIANCE)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_MAG_COVARIANCE];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.MAG_CAL)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_MAG_CAL];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.PROCESS_COVARIANCE)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_PROCESS_COVARIANCE];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.STATE_COVARIANCE)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_STATE_COVARIANCE];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }
            else if (index == (int)StateName.START_CAL)
            {
                AHRSPacket.PacketType = PID[(int)PName.GET_START_CAL];
                AHRSPacket.DataLength = 0;
                AHRSPacket.Data = new byte[0];

                if (!sendPacket(AHRSPacket))
                    return false;
            }

            return true;
        }

        /* **********************************************************************************
         * 
         * Function: private bool sendPacket
         * Inputs: Packet AHRSPacket - the packet to be transmitted
         * Outputs: None
         * Return Value: bool success
         * Dependencies: None
         * Description: 
         * 
         * Sends the specified packet to the AHRS.
         * 
         * Returns 'true' on success, 'false' otherwise
         * 
         * *********************************************************************************/
        private bool sendPacket( Packet AHRSPacket )
        {
            int i;
            UInt16 checksum;

            if (!connected)
                return false;

            byte[] packet = new byte[AHRSPacket.DataLength + 7];

            // Build packet header
            packet[0] = (byte)'s';
            packet[1] = (byte)'n';
            packet[2] = (byte)'p';
            packet[3] = AHRSPacket.PacketType;
            packet[4] = AHRSPacket.DataLength;

            // Fill data section
            for (i = 0; i < AHRSPacket.DataLength; i++)
            {
                packet[5 + i] = AHRSPacket.Data[i];
            }

            // Compute Checksum
            checksum = ComputeChecksum( packet, 5 + AHRSPacket.DataLength);

            // Add checksum to end of packet
            packet[5 + AHRSPacket.DataLength] = (byte)(checksum >> 8);
            packet[6 + AHRSPacket.DataLength] = (byte)(checksum & 0x0FF);

            // Now write the packet to the serial port
            try
            {
                serialPort.Write(packet, 0, AHRSPacket.DataLength + 7);
                
                PacketSentEvent((PName)getTypeIndex(AHRSPacket.PacketType), 0);
            }
            catch
            {
                return false;
            }

            return true;
        }

        /* **********************************************************************************
         * 
         * Function: private UInt16 ComputeChecksum
         * Inputs: byte packet
         * Outputs: None
         * Return Value: A two-byte checksum
         * Dependencies: None
         * Description: 
         * 
         * Computes the sum of all bytes in the packet and returns a two byte value.
         * 
         * 
         * *********************************************************************************/
        private UInt16 ComputeChecksum(byte[] packet, int length)
        {
            UInt16 sum = 0;
            int i;

            for (i = 0; i < length; i++)
            {
                sum += packet[i];
            }

            return sum;
        }

        /* **********************************************************************************
         * 
         * Function: public void ResetToFactory
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to change all settings back to factory settings.
         * 
         * *********************************************************************************/
        public void ResetToFactory()
        {
            UpdatePending[(int)StateName.RESET_TO_FACTORY] = true;

            synch();
        }

        /* **********************************************************************************
         * 
         * Function: public void ZeroRateGyros
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to begin a ZERO_RATE_GYROS command
         * 
         * *********************************************************************************/
        public void ZeroRateGyros()
        {
            UpdatePending[(int)StateName.ZERO_GYROS] = true;

            synch();
        }

        /* **********************************************************************************
         * 
         * Function: public void AutoSetMagRef
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to set the current magnetic field measurement as the zero yaw
         * reference angle
         * 
         * *********************************************************************************/
        public void AutoSetMagRef()
        {
            UpdatePending[(int)StateName.AUTO_SET_MAG_REF] = true;

            synch();
        }

        /* **********************************************************************************
         * 
         * Function: public void AutoSetAccelRef
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to set the current accel measurement as the zero pitch and roll
         * reference angle
         * 
         * *********************************************************************************/
        public void AutoSetAccelRef()
        {
            UpdatePending[(int)StateName.AUTO_SET_ACCEL_REF] = true;

            synch();
        }


        /* **********************************************************************************
         * 
         * Function: public void SelfTest
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to execute a SELF_TEST command
         * 
         * *********************************************************************************/
        public void SelfTest()
        {
            UpdatePending[(int)StateName.SELF_TEST] = true;

            synch();
        }

        /* **********************************************************************************
         * 
         * Function: public void GetStateCovariance
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to send a STATE_COVARIANCE_REPORT packet
         * 
         * *********************************************************************************/
        public void GetStateCovariance()
        {
            Measured[(int)StateName.STATE_COVARIANCE] = false;

            synch();
        }

        /* **********************************************************************************
         * 
         * Function: public void WriteToFlash
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to write its current settings into FLASH
         * 
         * *********************************************************************************/
        public void WriteToFlash()
        {
            UpdatePending[(int)StateName.WRITE_TO_FLASH] = true;

            synch();
        }

        /* **********************************************************************************
         * 
         * Function: public void EKF_Reset
         * Inputs: None
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Causes the AHRS to reset the onboard EKF
         * 
         * *********************************************************************************/
        public void EKF_Reset()
        {
            UpdatePending[(int)StateName.EKF_RESET] = true;

            synch();
        }

    }
}
