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

mc-auto: handle EKF heading reset

Browse Source
This commit is contained in:
bresch
2025-09-03 14:36:51 +02:00
committed by Mathieu Bresciani
parent 82308da18d
commit c06ba93175
3 changed files with 10 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+5 -4
View File
@@ -707,19 +707,20 @@ void FlightTaskAuto::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vx
_position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN});
}
void FlightTaskAuto::_ekfResetHandlerPositionZ(float delta_z)
void FlightTaskAuto::_ekfResetHandlerPositionZ(const float delta_z)
{
_position_smoothing.forceSetPosition({NAN, NAN, _position(2)});
}
void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz)
void FlightTaskAuto::_ekfResetHandlerVelocityZ(const float delta_vz)
{
_position_smoothing.forceSetVelocity({NAN, NAN, _velocity(2)});
}
void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
void FlightTaskAuto::_ekfResetHandlerHeading(const float delta_psi)
{
_yaw_setpoint_previous += delta_psi;
_yaw_setpoint_previous = wrap_pi(_yaw_setpoint_previous + delta_psi);
_heading_smoothing.reset(wrap_pi(_heading_smoothing.getSmoothedHeading() + delta_psi));
}
void FlightTaskAuto::_checkEmergencyBraking()