dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: report dist bottom also when using flow for terrain 

Terrain height can be estimated using a range finder and/or optical flow
This commit is contained in:
bresch 2023-07-03 13:34:20 +02:00 committed by Daniel Agar
parent 36ec1fa811
commit a07b8c08ab
3 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -760,9 +760,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
#if defined(CONFIG_EKF2_RANGE_FINDER)
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
#endif // CONFIG_EKF2_RANGE_FINDER
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
@ -1026,15 +1024,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
void Ekf::updateGroundEffect()
{
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (isTerrainEstimateValid()) {
// automatically set ground effect if terrain is valid
float height = _terrain_vpos - _state.pos(2);
_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
} else
#endif // CONFIG_EKF2_RANGE_FINDER
if (_control_status.flags.gnd_effect) {
} else if (_control_status.flags.gnd_effect) {
// Turn off ground effect compensation if it times out
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
_control_status.flags.gnd_effect = false;

2
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -415,10 +415,12 @@ void Ekf::controlHaglFakeFusion()
bool Ekf::isTerrainEstimateValid() const
{
#if defined(CONFIG_EKF2_RANGE_FINDER)
// we have been fusing range finder measurements in the last 5 seconds
if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
return true;
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds

7
src/modules/ekf2/EKF2.cpp
View File

@ -1462,13 +1462,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
lpos.delta_heading = Eulerf(delta_q_reset).psi();
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
#if defined(CONFIG_EKF2_RANGE_FINDER)
// Distance to bottom surface (ground) in meters
// constrain the distance to ground to _rng_gnd_clearance
lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
// Distance to bottom surface (ground) in meters, must be positive
lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
#endif // CONFIG_EKF2_RANGE_FINDER
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);