mirror of https://github.com/ArduPilot/ardupilot
removed redundant FP calcs for speed up.
added ability to alternate normalization and drift correction. Not fully implemented, needs further testing. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2691 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
33362a86c2
commit
db0e5760a4
|
@ -48,8 +48,13 @@ AP_DCM::update_DCM(float _G_Dt)
|
|||
_accel_vector = _imu->get_accel(); // Get current values for IMU sensors
|
||||
|
||||
matrix_update(_G_Dt); // Integrate the DCM matrix
|
||||
|
||||
//if(_toggle){
|
||||
normalize(); // Normalize the DCM matrix
|
||||
//}else{
|
||||
drift_correction(); // Perform drift correction
|
||||
//}
|
||||
//_toggle = !_toggle;
|
||||
|
||||
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
|
||||
}
|
||||
|
@ -92,14 +97,20 @@ AP_DCM::matrix_update(float _G_Dt)
|
|||
}
|
||||
|
||||
#if OUTPUTMODE == 1
|
||||
float tmp = _G_Dt * _omega.x;
|
||||
update_matrix.b.z = -tmp; // -delta Theta x
|
||||
update_matrix.c.y = tmp; // delta Theta x
|
||||
|
||||
tmp = _G_Dt * _omega.y;
|
||||
update_matrix.c.x = -tmp; // -delta Theta y
|
||||
update_matrix.a.z = tmp; // delta Theta y
|
||||
|
||||
tmp = _G_Dt * _omega.z;
|
||||
update_matrix.b.x = tmp; // delta Theta z
|
||||
update_matrix.a.y = -tmp; // -delta Theta z
|
||||
|
||||
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;
|
||||
|
@ -115,7 +126,6 @@ AP_DCM::matrix_update(float _G_Dt)
|
|||
|
||||
temp_matrix = _dcm_matrix * update_matrix;
|
||||
_dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -33,7 +33,8 @@ public:
|
|||
_health(1.),
|
||||
_kp_roll_pitch(0.05967),
|
||||
_ki_roll_pitch(0.00001278),
|
||||
_kp_yaw(0.8)
|
||||
_kp_yaw(0.8),
|
||||
_toggle(0)
|
||||
{}
|
||||
|
||||
// Accessors
|
||||
|
@ -115,6 +116,7 @@ private:
|
|||
float _course_over_ground_y; // Course overground Y axis
|
||||
float _health;
|
||||
bool _centripetal;
|
||||
bool _toggle;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue