int gps_fix_count = 5; //used to count 5 good fixes at ground startup public DCM(nIMU imu) { this.adc = imu; DCM_Matrix = new float[3][]; DCM_Matrix[0] = new float[] { 1, 0, 0 }; DCM_Matrix[1] = new float[] { 0, 1, 0 }; DCM_Matrix[2] = new float[] { 0, 0, 1 }; Temporary_Matrix = new float[3][]; Temporary_Matrix[0] = new float[] { 0, 0, 0 }; Temporary_Matrix[1] = new float[] { 0, 0, 0 }; Temporary_Matrix[2] = new float[] { 0, 0, 0 }; Update_Matrix = new float[3][]; Update_Matrix[0] = new float[] { 0, 1, 2 }; Update_Matrix[1] = new float[] { 3, 4, 5 }; Update_Matrix[2] = new float[] { 6, 7, 8 }; }
float[][] Update_Matrix; // = { new float[] { 0, 1, 2 }, new float[] { 3, 4, 5 }, new float[] { 6, 7, 8 } }; //Gyros here #endregion Fields #region Constructors public DCM(nIMU imu) { this.adc = imu; DCM_Matrix = new float[3][]; DCM_Matrix[0] = new float[] { 1, 0, 0 }; DCM_Matrix[1] = new float[] { 0, 1, 0 }; DCM_Matrix[2] = new float[] { 0, 0, 1 }; Temporary_Matrix = new float[3][]; Temporary_Matrix[0] = new float[] { 0, 0, 0 }; Temporary_Matrix[1] = new float[] { 0, 0, 0 }; Temporary_Matrix[2] = new float[] { 0, 0, 0 }; Update_Matrix = new float[3][]; Update_Matrix[0] = new float[] { 0, 1, 2 }; Update_Matrix[1] = new float[] { 3, 4, 5 }; Update_Matrix[2] = new float[] { 6, 7, 8 }; }