dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: fix resetGlobalPosToExternalObservation whitespace

This commit is contained in:
Daniel Agar 2024-01-11 10:55:50 -05:00
parent 5c845a58f1
commit 8bcba6128a
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf2/EKF/ekf.cpp
View File

@ -341,20 +341,21 @@ void Ekf::predictState(const imuSample &imu_delayed)
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
}
void
Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) {
void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
{
if (!_pos_ref.isInitialized()) {
return;
}
if (!_pos_ref.isInitialized()) {
return;
}
// apply a first order correction using velocity at the delated time horizon and the delta time
timestamp_observation = math::min(_time_latest_us, timestamp_observation);
const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation) * 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;
// apply a first order correction using velocity at the delated time horizon and the delta time
timestamp_observation = math::min(_time_latest_us, timestamp_observation);
const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation)
* 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
}
void Ekf::updateParameters()

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

@ -502,7 +502,7 @@ public:
return is_healthy;
}
void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
void updateParameters();
@ -814,7 +814,7 @@ private:
void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
void resetHorizontalPositionToLastKnown();
void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy);
void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }