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

ekf2: resetHeightTo -> resetAltitudeTo

Browse Source 
The vertical position state is now an altitude, not just a local height
This commit is contained in:
bresch 2024-11-22 12:21:16 +01:00 committed by Mathieu Bresciani
parent 93c690f133
commit 14fe6c2167
8 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View File

@ -131,7 +131,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_baro = true;
resetHeightTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
// reset vertical velocity if no valid sources available

6
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
View File

@ -117,7 +117,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
if (_height_sensor_ref == HeightSensor::EV) {
_information_events.flags.reset_hgt_to_ev = true;
resetHeightTo(-measurement, measurement_var);
resetAltitudeTo(-measurement, measurement_var);
bias_est.reset();
} else {
@ -146,7 +146,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
// All height sources are failing
ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetHeightTo(-measurement - bias_est.getBias(), measurement_var);
resetAltitudeTo(-measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(_gpos.altitude() + measurement);
aid_src.time_last_fuse = _time_delayed_us;
@ -170,7 +170,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::EV)) {
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetHeightTo(-measurement, measurement_var);
resetAltitudeTo(-measurement, measurement_var);
_height_sensor_ref = HeightSensor::EV;
bias_est.reset();

2
src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp
View File

@ -110,7 +110,7 @@ void Ekf::resetHeightToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
resetHeightTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
}
void Ekf::stopFakeHgtFusion()

2
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@ -105,7 +105,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_gps = true;
resetHeightTo(measurement, measurement_var);
resetAltitudeTo(measurement, measurement_var);
bias_est.setBias(-_gpos.altitude() + measurement);
aid_src.time_last_fuse = _time_delayed_us;

4
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -148,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_height_sensor_ref = HeightSensor::RANGE;
_information_events.flags.reset_hgt_to_rng = true;
resetHeightTo(aid_src.observation, aid_src.observation_variance);
resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
_state.terrain = 0.f;
_control_status.flags.rng_hgt = true;
stopRngTerrFusion();
@ -180,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_rng = true;
resetHeightTo(aid_src.observation - _state.terrain);
resetAltitudeTo(aid_src.observation - _state.terrain);
// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {

2
src/modules/ekf2/EKF/ekf.h
View File

@ -725,7 +725,7 @@ private:
bool isHeightResetRequired() const;
void resetHeightTo(float new_altitude, float new_vert_pos_var = NAN);
void resetAltitudeTo(float new_altitude, float new_vert_pos_var = NAN);
void updateVerticalPositionResetStatus(const float delta_z);
void resetVerticalVelocityToZero();

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

@ -137,7 +137,7 @@ bool Ekf::setAltOrigin(const float altitude, const float vpos_var)
if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) {
const float local_alt_prev = _gpos.altitude();
_local_origin_alt = altitude;
resetHeightTo(local_alt_prev + _local_origin_alt);
resetAltitudeTo(local_alt_prev + _local_origin_alt);
} else {
const float delta_origin_alt = altitude - _local_origin_alt;
@ -235,7 +235,7 @@ bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var)
ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt);
}
resetHeightTo(altitude, vpos_var);
resetAltitudeTo(altitude, vpos_var);
return true;
}

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

@ -182,7 +182,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos,
resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var);
}
void Ekf::resetHeightTo(const float new_altitude, float new_vert_pos_var)
void Ekf::resetAltitudeTo(const float new_altitude, float new_vert_pos_var)
{
const float old_altitude = _gpos.altitude();
_gpos.setAltitude(new_altitude);