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

Merge pull request #305 from PX4/revert-303-pr-get_terr_valid

Browse Source 
Revert "Split get_terrain_vert_pos() into valid and get_vpos"
This commit is contained in:
Paul Riseborough 2017-07-19 17:34:03 +10:00 committed by GitHub
commit 13f8d949bd
5 changed files with 18 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/control.cpp
View File

@ -734,7 +734,7 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (get_terrain_valid()) {
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
@ -774,7 +774,7 @@ void Ekf::controlHeightFusion()
// measurment 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 && _terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
@ -807,7 +807,7 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
if (get_terrain_valid()) {
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
@ -875,12 +875,12 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool use_range_finder;
if (in_range_aid_mode) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised;
} else {
// if we were not using range aid in the previous iteration then require the current height above terrain to be
// smaller than 70 % of the maximum allowed ground distance for range aid
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid();
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised;
}
bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)

8
EKF/ekf.h
View File

@ -154,11 +154,9 @@ public:
// return true if the EKF is dead reckoning the position using inertial data only
bool inertial_dead_reckoning();
// return true if the terrain estimate is valid
bool get_terrain_valid();
// get the estimated terrain vertical position relative to the NED origin
void get_terrain_vert_pos(float *ret);
// return true if the etimate is valid
// return the estimated terrain vertical position relative to the NED origin
bool get_terrain_vert_pos(float *ret);
// get the accerometer bias in m/s/s
void get_accel_bias(float bias[3]);

3
EKF/ekf_helper.cpp
View File

@ -1040,7 +1040,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
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;
soln_status.flags.pos_vert_agl = get_terrain_valid();
float dummy_var;
soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var);
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_vert_abs;

8
EKF/estimator_interface.h
View File

@ -199,11 +199,9 @@ public:
// return true if the EKF is dead reckoning the position using inertial data only
virtual bool inertial_dead_reckoning() = 0;
// return true if the terrain estimate is valid
virtual bool get_terrain_valid() = 0;
// get the estimated terrain vertical position relative to the NED origin
virtual void get_terrain_vert_pos(float *ret) = 0;
// return true if the estimate is valid
// return the estimated terrain vertical position relative to the NED origin
virtual bool get_terrain_vert_pos(float *ret) = 0;
// return true if the local position estimate is valid
bool local_position_is_valid();

13
EKF/terrain_estimator.cpp
View File

@ -150,9 +150,12 @@ void Ekf::fuseHagl()
}
}
// return true if the terrain estimate is valid
bool Ekf::get_terrain_valid()
// return true if the estimate is fresh
// return the estimated vertical position of the terrain relative to the NED origin
bool Ekf::get_terrain_vert_pos(float *ret)
{
memcpy(ret, &_terrain_vpos, sizeof(float));
if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) {
return true;
@ -161,12 +164,6 @@ bool Ekf::get_terrain_valid()
}
}
// get the estimated vertical position of the terrain relative to the NED origin
void Ekf::get_terrain_vert_pos(float *ret)
{
memcpy(ret, &_terrain_vpos, sizeof(float));
}
void Ekf::get_hagl_innov(float *hagl_innov)
{
memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));