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

Terrain estimator: formatting and remove redundant comments

Browse Source
This commit is contained in:
bresch
2019-10-10 16:36:48 +02:00
committed by Paul Riseborough
parent 0aef0eda59
commit b38458c2ab
5 changed files with 40 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+6 -6
View File
@@ -502,7 +502,7 @@ void Ekf::controlOpticalFlowFusion()
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude
&& !_inhibit_flow_use
&& get_terrain_valid()) // we have a valid distance to ground estimate
&& isTerrainEstimateValid())
{
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
@@ -1023,7 +1023,7 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (get_terrain_valid()) {
if (isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
} else {
@@ -1070,7 +1070,7 @@ void Ekf::controlHeightFusion()
// measurement matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
if (_control_status.flags.in_air && get_terrain_valid()) {
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
@@ -1105,7 +1105,7 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (get_terrain_valid()) {
if (isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
} else {
@@ -1186,7 +1186,7 @@ void Ekf::checkRangeAidSuitability()
if (_control_status.flags.in_air
&& !_rng_hgt_faulty
&& get_terrain_valid()
&& isTerrainEstimateValid()
&& horz_vel_valid) {
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
@@ -1417,7 +1417,7 @@ void Ekf::controlMagFusion()
if (!_control_status.flags.mag_align_complete && _control_status.flags.in_air) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested.
float terrain_vpos_estimate = get_terrain_valid() ? _terrain_vpos : _last_on_ground_posD;
float terrain_vpos_estimate = isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
_mag_yaw_reset_req |= (terrain_vpos_estimate - _state.pos(2)) > 1.5f;
}