Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DCM: only add in centripetal accel if we have GPS lock 

if we don't have a GPS or the GPS doesn't have a good lock then we
can't rely on the ground speed for adjusting the acceleration vector
This commit is contained in:
Andrew Tridgell 2012-02-18 17:44:31 +11:00
parent ffa3330f93
commit d52cb7e574
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_DCM/AP_DCM.cpp
View File

@ -131,8 +131,11 @@ AP_DCM::matrix_update(float _G_Dt)
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega) _omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){ if(_centripetal &&
accel_adjust(); // Remove _centripetal acceleration. _gps != NULL &&
_gps->status() == GPS::GPS_OK) {
// Remove _centripetal acceleration.
accel_adjust();
} }
#if OUTPUTMODE == 1 #if OUTPUTMODE == 1
@ -174,9 +177,7 @@ AP_DCM::accel_adjust(void)
{ {
Vector3f veloc, temp; Vector3f veloc, temp;
if (_gps) { veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
}
// We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive // We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive