fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning

Browse Source 
and increase horizontal velocity variance to allow velocity states to move
towards solution that is aligned with the newly set wind

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2024-07-03 18:36:21 +02:00
parent ec5f09d5cb
commit 16cf77da38
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF/wind.cpp
View File

@ -60,6 +60,11 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction,
_information_events.flags.reset_wind_to_ext_obs = true;
resetWindTo(wind, wind_var);
// reset the horizontal velocity variances to allow the velocity states to be pulled towards
// a solution that is aligned with the newly set wind estimates
static constexpr float hor_vel_var = 25.0f;
P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var);
}
void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)

6
src/modules/ekf2/EKF2.cpp
View File

@ -536,8 +536,10 @@ void EKF2::Run()
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
vehicle_command.param4);
if (_ekf.control_status_flags().wind_dead_reckoning) {
_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
vehicle_command.param4);
}
}
}
}