diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 70cb041784bb..c645afd9d5c7 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -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 diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index f62be7575fdc..232ecd375284 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -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(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(); diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index 4ff4e67d6cd9..8fa2dfb62592 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -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() diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index d31587461570..c936442b1405 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -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; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 2598f3a3318c..fec1fd9d8e85 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -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()) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 41728ea0084f..f42493652ecc 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -725,7 +725,7 @@ class Ekf final : public EstimatorInterface 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(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e9332fdd901b..9638abeb694c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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; } diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index e4c16b49415b..7d874e654081 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -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);