UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-30 10:58:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: immediately switch out of goto setpoint when receiving trajectory_setpoint

Browse Source 
Previously, when switching from a goto setpoint into a mode that publishes
trajectory_setpoint, the previous goto setpoint was still used for 500ms,
which then caused a setpoint jump.
This change makes sure that when a trajectory_setpoint is received, any
existing goto setpoint is marked as invalid immediately.
This commit is contained in:
Beat Küng
2025-05-21 15:12:05 +02:00
committed by Silvan Fuhrer
parent f0ecd9e757
commit 80febaf0f0
3 changed files with 25 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@@ -426,9 +426,12 @@ void MulticopterPositionControl::Run()
PositionControlStates states{set_vehicle_states(vehicle_local_position, dt)};
// if a goto setpoint available this publishes a trajectory setpoint to go there
if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample,
_vehicle_control_mode.flag_multicopter_position_control_enabled)) {
// If a goto setpoint is available this publishes a trajectory setpoint to go there
// If trajectory_setpoint is published elsewhere, do not use the goto setpoint
const bool goto_setpoint_enable = _vehicle_control_mode.flag_multicopter_position_control_enabled
&& !_trajectory_setpoint_sub.updated();
if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, goto_setpoint_enable)) {
_goto_control.update(dt, states.position, states.yaw);
}