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

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

@ -341,8 +341,8 @@ 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;
@ -350,7 +350,8 @@ Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float a
// 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;
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;