Browse Source

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

master
Paul Riseborough 11 years ago committed by Andrew Tridgell
parent
commit
bc311542ab
  1. 18
      libraries/AP_TECS/AP_TECS.cpp

18
libraries/AP_TECS/AP_TECS.cpp

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

Loading…
Cancel
Save