Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

format update
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Details

Browse Source
This commit is contained in:
henrykotze
2026-05-17 18:50:23 +02:00
parent 26dc1085a0
commit ffd654de51
2 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
+8 -5
View File
@@ -222,11 +222,13 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
ECL_INFO("starting %s fusion, resetting state", EV_AID_SRC_NAME);
//_position_sensor_ref = PositionSensor::EV;
_information_events.flags.reset_pos_to_vision = true;
if(_ev_pos_b_est.wasFusionActive()){
if (_ev_pos_b_est.wasFusionActive()) {
// if we used a bias before, lets make use of it again to align our sensor.
// We are not activing fusion of the bias, because we do not have a another sensor aiding position
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
}else{
} else {
resetHorizontalPositionTo(measurement, measurement_var);
_ev_pos_b_est.reset();
}
@@ -273,7 +275,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
}
// Check if external vision (EV) position fusion has failed to occur within the allowed timeout period.
// If it has timed out, velocity drift is no longer constrained, indicating a fusion failure.
// If it has timed out, velocity drift is no longer constrained, indicating a fusion failure.
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second
if (is_fusion_failing) {
@@ -297,8 +299,9 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
// are aligned
_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
// Our position state hasnt been constrained by any sensor measurement.
// we thus reset our position state using our EV position measurement
// Our position state hasnt been constrained by any sensor measurement.
// we thus reset our position state using our EV position measurement
} else {
_information_events.flags.reset_pos_to_vision = true;
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
src/modules/ekf2/EKF/bias_estimator/position_bias_estimator.hpp
+3 -1
View File
@@ -50,8 +50,9 @@ public:
virtual ~PositionBiasEstimator() = default;
bool fusionActive() const { return _is_sensor_fusion_active; }
bool wasFusionActive() const { return _was_fusion_active; }
void setFusionActive() { _is_sensor_fusion_active = true; }
void setFusionActive() { _is_sensor_fusion_active = _was_fusion_active = true; }
void setFusionInactive() { _is_sensor_fusion_active = false; }
void predict(float dt)
@@ -111,6 +112,7 @@ private:
const PositionSensor &_sensor_ref;
bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool
bool _was_fusion_active{false};
};
#endif // !EKF_POSITION_BIAS_ESTIMATOR_HPP