fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added support to reset vehicle position based on external position (#22444)

Browse Source 
via command MAV_CMD_EXTERNAL_POSITION_ESTIMATE

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst
2024-01-11 13:09:22 +01:00
committed by GitHub
parent d45c3d3407
commit 603c3f6636
6 changed files with 51 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+16
View File
@@ -341,6 +341,22 @@ 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) {
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;
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
}
void Ekf::updateParameters()
{
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)