fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 18:19:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated logic of when to fuse beta. Now synthetic sideslip measurements are fused after 5 seconds of cruise flight with airspeed sensor activated

Browse Source
This commit is contained in:
CarlOlsson 2016-08-23 20:15:03 +02:00 committed by Lorenz Meier
parent dec686afd7
commit e1d9d503e3
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
EKF/control.cpp
View File

@ -751,8 +751,11 @@ void Ekf::controlRangeFinderFusion()
void Ekf::controlAirDataFusion()
{
// control activation and initialisation/reset of wind states
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
// wind states are required to do airspeed fusion
if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
if ((airspeed_timed_out && !_control_status.flags.fuse_beta) || _time_last_arsp_fuse == 0) {
// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
_control_status.flags.wind = false;
@ -776,10 +779,15 @@ void Ekf::controlBetaFusion()
{
bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us;
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air){
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && _time_last_imu - _time_last_arsp_fuse > 5e6){
fuseSideslip();
}
_control_status.flags.wind = true; // Look into this
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
if(sideslip_timed_out && _control_status.flags.fuse_beta){
_control_status.flags.wind = false;
}
}