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

EKF2: use fixed time constant for GPS blending

Browse Source 
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson
2020-02-28 10:24:27 +01:00
committed by Mathieu Bresciani
parent 357700aa8d
commit 8de1f5229b
1 changed files with 2 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+2 -11
View File
@@ -2209,7 +2209,6 @@ void Ekf2::update_gps_offsets()
{
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
// Increase the filter time constant proportional to the inverse of the weighting
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
float alpha[GPS_MAX_RECEIVERS] = {};
float omega_lpf = 1.0f / fmaxf(_param_ekf2_gps_tau.get(), 1.0f);
@@ -2217,16 +2216,8 @@ void Ekf2::update_gps_offsets()
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].time_usec - _time_prev_us[i] > 0) {
// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]),
0.0f, 1.0f);
// scale the filter coefficient so that time constant is inversely proprtional to weighting
if (_blend_weights[i] > min_alpha) {
alpha[i] = min_alpha / _blend_weights[i];
} else {
alpha[i] = 1.0f;
}
alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]),
0.0f, 1.0f);
_time_prev_us[i] = _gps_state[i].time_usec;
}