void AP_DCM_FW::matrix_update(float _G_Dt) { Matrix3f _update_matrix; static int8_t timer; //Record when you saturate any of the gyros. if((abs(_gyro_vector.x) >= radians(300)) || (abs(_gyro_vector.y) >= radians(300)) || (abs(_gyro_vector.z) >= radians(300))) gyro_sat_count++; _omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega) _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms _accel_adjust(); // Remove centrifugal acceleration. #if OUTPUTMODE == 1 _update_matrix.a.x = 0; _update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z _update_matrix.a.z = _G_Dt * _omega.y; // delta Theta y _update_matrix.b.x = _G_Dt * _omega.z; // delta Theta z _update_matrix.b.y = 0; _update_matrix.b.z = -_G_Dt * _omega.x; // -delta Theta x _update_matrix.c.x = -_G_Dt * _omega.y; // -delta Theta y _update_matrix.c.y = _G_Dt * _omega.x; // delta Theta x _update_matrix.c.z = 0; #else // Uncorrected data (no drift correction) _update_matrix.a.x = 0; _update_matrix.a.y = -_G_Dt * _gyro_vector.z; _update_matrix.a.z = _G_Dt * _gyro_vector.y; _update_matrix.b.x = _G_Dt * _gyro_vector.z; _update_matrix.b.y = 0; _update_matrix.b.z = -_G_Dt * _gyro_vector.x; _update_matrix.c.x = -_G_Dt * _gyro_vector.y; _update_matrix.c.y = _G_Dt * _gyro_vector.x; _update_matrix.c.z = 0; #endif Serial.println("update matrix before"); Serial.println(_update_matrix.a.x); Serial.println(_update_matrix.a.y); Serial.println(_update_matrix.a.z); Serial.println(_update_matrix.b.x); Serial.println(_update_matrix.b.y); Serial.println(_update_matrix.b.z); Serial.println(_update_matrix.c.x); Serial.println(_update_matrix.c.y); Serial.println(_update_matrix.c.z); Serial.println("dcm matrix before"); Serial.println(_dcm_matrix.a.x); Serial.println(_dcm_matrix.a.y); Serial.println(_dcm_matrix.a.z); Serial.println(_dcm_matrix.b.x); Serial.println(_dcm_matrix.b.y); Serial.println(_dcm_matrix.b.z); Serial.println(_dcm_matrix.c.x); Serial.println(_dcm_matrix.c.y); Serial.println(_dcm_matrix.c.z); // update _update_matrix = _dcm_matrix * _update_matrix; // Equation 17 Serial.println("update matrix middle"); Serial.println(_update_matrix.a.x,12); Serial.println(_update_matrix.a.y,12); Serial.println(_update_matrix.a.z,12); Serial.println(_update_matrix.b.x,12); Serial.println(_update_matrix.b.y,12); Serial.println(_update_matrix.b.z,12); Serial.println(_update_matrix.c.x,12); Serial.println(_update_matrix.c.y,12); Serial.println(_update_matrix.c.z,12); Serial.println("dcm matrix middle"); Serial.println(_dcm_matrix.a.x,12); Serial.println(_dcm_matrix.a.y,12); Serial.println(_dcm_matrix.a.z,12); Serial.println(_dcm_matrix.b.x,12); Serial.println(_dcm_matrix.b.y,12); Serial.println(_dcm_matrix.b.z,12); Serial.println(_dcm_matrix.c.x,12); Serial.println(_dcm_matrix.c.y,12); Serial.println(_dcm_matrix.c.z,12); _dcm_matrix = _dcm_matrix + _update_matrix; // Equation 17 Serial.println("update matrix after"); Serial.println(_update_matrix.a.x); Serial.println(_update_matrix.a.y); Serial.println(_update_matrix.a.z); Serial.println(_update_matrix.b.x); Serial.println(_update_matrix.b.y); Serial.println(_update_matrix.b.z); Serial.println(_update_matrix.c.x); Serial.println(_update_matrix.c.y); Serial.println(_update_matrix.c.z); Serial.println("dcm matrix after"); Serial.println(_dcm_matrix.a.x); Serial.println(_dcm_matrix.a.y); Serial.println(_dcm_matrix.a.z); Serial.println(_dcm_matrix.b.x); Serial.println(_dcm_matrix.b.y); Serial.println(_dcm_matrix.b.z); Serial.println(_dcm_matrix.c.x); Serial.println(_dcm_matrix.c.y); Serial.println(_dcm_matrix.c.z); }
void AP_DCM::matrix_update(void) { DCM_Matrix update_matrix; _gyro_vector(0) = gyro_scaled_X(read_adc(0)); // gyro x roll _gyro_vector(1) = gyro_scaled_Y(read_adc(1)); // gyro y pitch _gyro_vector(2) = gyro_scaled_Z(read_adc(2)); // gyro Z yaw //Record when you saturate any of the gyros. if((abs(_gyro_vector(0)) >= radians(300)) || (abs(_gyro_vector(1)) >= radians(300)) || (abs(_gyro_vector(2)) >= radians(300))) gyro_sat_count++; /* Serial.print (__adc_in[0]); Serial.print (" "); Serial.print (_adc_offset[0]); Serial.print (" "); Serial.print (_gyro_vector(0)); Serial.print (" "); Serial.print (__adc_in[1]); Serial.print (" "); Serial.print (_adc_offset[1]); Serial.print (" "); Serial.print (_gyro_vector(1)); Serial.print (" "); Serial.print (__adc_in[2]); Serial.print (" "); Serial.print (_adc_offset[2]); Serial.print (" "); Serial.println (_gyro_vector(2)); */ // _accel_vector(0) = read_adc(3); // acc x // _accel_vector(1) = read_adc(4); // acc y // _accel_vector(2) = read_adc(5); // acc z // Low pass filter on accelerometer data (to filter vibrations) _accel_vector(0) = _accel_vector(0) * 0.6 + (float)read_adc(3) * 0.4; // acc x _accel_vector(1) = _accel_vector(1) * 0.6 + (float)read_adc(4) * 0.4; // acc y _accel_vector(2) = _accel_vector(2) * 0.6 + (float)read_adc(5) * 0.4; // acc z _omega = _gyro_vector + _omega_I; // adding proportional term _omega_vector = _omega + _omega_P; // adding Integrator term _accel_adjust(); // Remove centrifugal acceleration. #if OUTPUTMODE == 1 update_matrix(0, 0) = 0; update_matrix(0, 1) = -_G_Dt * _omega_vector(2); // -z update_matrix(0, 2) = _G_Dt * _omega_vector(1); // y update_matrix(1, 0) = _G_Dt * _omega_vector(2); // z update_matrix(1, 1) = 0; update_matrix(1, 2) = -_G_Dt * _omega_vector(0); // -x update_matrix(2, 0) = -_G_Dt * _omega_vector(1); // -y update_matrix(2, 1) = _G_Dt * _omega_vector(0); // x update_matrix(2, 2) = 0; #else // Uncorrected data (no drift correction) update_matrix(0, 0) = 0; update_matrix(0, 1) = -_G_Dt * _gyro_vector(2); // -z update_matrix(0, 2) = _G_Dt * _gyro_vector(1); // y update_matrix(1, 0) = _G_Dt * _gyro_vector(2); // z update_matrix(1, 1) = 0; update_matrix(1, 2) = -_G_Dt * _gyro_vector(0); update_matrix(2, 0) = -_G_Dt * _gyro_vector(1); update_matrix(2, 1) = _G_Dt * _gyro_vector(0); update_matrix(2, 2) = 0; #endif // update _dcm_matrix += _dcm_matrix * update_matrix; /* Serial.print (_G_Dt * 1000); Serial.print (" "); Serial.print (dcm_matrix(0, 0)); Serial.print (" "); Serial.print (dcm_matrix(0, 1)); Serial.print (" "); Serial.print (dcm_matrix(0, 2)); Serial.print (" "); Serial.print (dcm_matrix(1, 0)); Serial.print (" "); Serial.print (dcm_matrix(1, 1)); Serial.print (" "); Serial.print (dcm_matrix(1, 2)); Serial.print (" "); Serial.print (dcm_matrix(2, 0)); Serial.print (" "); Serial.print (dcm_matrix(2, 1)); Serial.print (" "); Serial.println (dcm_matrix(2, 2)); */ }