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));
*/
}
