Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS : Fixed bug preventing accel launch detection when not using AS sensor

Browse Source
This commit is contained in:
Paul Riseborough 2014-02-07 19:08:33 +11:00 committed by Andrew Tridgell
parent 91b78c0cbe
commit bc311542ab
1 changed files with 6 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_TECS/AP_TECS.cpp
View File

@ -170,18 +170,12 @@ void AP_TECS::update_50hz(float hgt_afe)
}
// Update and average speed rate of change
// Only required if airspeed is being measured and controlled
float temp = 0;
if (_ahrs.airspeed_sensor_enabled() && _ahrs.airspeed_estimate_true(&_EAS)) {
// Get DCM
const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Calculate speed rate of change
temp = rotMat.c.x * GRAVITY_MSS + _ahrs.get_ins().get_accel().x;
// take 5 point moving average
_vel_dot = _vdot_filter.apply(temp);
} else {
_vel_dot = 0.0f;
}
// Get DCM
const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Calculate speed rate of change
float temp = rotMat.c.x * GRAVITY_MSS + _ahrs.get_ins().get_accel().x;
// take 5 point moving average
_vel_dot = _vdot_filter.apply(temp);
}