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

addressed review comments

Browse Source
This commit is contained in:
Roman 2019-05-12 18:33:03 +02:00 committed by Paul Riseborough
parent 8673ab0346
commit c085d7295d
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -124,7 +124,7 @@ void Ekf::controlFusionModes()
}
// check if we should fuse flow data for terrain estimation
if (!_flow_for_terrain_data_ready && _flow_data_ready) {
if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
// only fuse flow for terrain if range data hasn't been fused for 5 seconds
_flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000;
// only fuse flow for terrain if the main filter is not fusing flow and we are using gps

6
EKF/terrain_estimator.cpp
View File

@ -56,7 +56,8 @@ bool Ekf::initHagl()
// success
return true;
} else if (_flow_for_terrain_data_ready) {
_terrain_vpos = _state.pos(2);
// initialise terrain vertical position to origin as this is the best guess we have
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
_terrain_var = 100.0f;
return true;
} else if (!_control_status.flags.in_air) {
@ -214,6 +215,9 @@ void Ekf::fuseFlowForTerrain()
// Calculate observation matrix for flow around the vehicle x axis
float Hx = vel_body(1) * t0 /(pred_hagl * pred_hagl);
// Constrain terrain variance to be non-negative
_terrain_var = fmaxf(_terrain_var, 0.0f);
// Cacluate innovation variance
_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;