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

9
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 = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){
accel_adjust(); // Remove _centripetal acceleration.
if(_centripetal &&
_gps != NULL &&
_gps->status() == GPS::GPS_OK) {
// Remove _centripetal acceleration.
accel_adjust();
}
#if OUTPUTMODE == 1
@ -174,9 +177,7 @@ AP_DCM::accel_adjust(void)
{
Vector3f veloc, temp;
if (_gps) {
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