From 684e2974ac042e6ba2168f06bf91be4fbb7b6050 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 27 Feb 2019 10:21:22 +0100 Subject: [PATCH] mc_pos_control_main - Re-add updateVelocityControllerIO for glitch-free position unlock --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a56116ced6..6294148729 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -777,6 +777,10 @@ MulticopterPositionControl::run() // vehicle intention. publish_local_pos_sp(local_pos_sp); + // Inform FlightTask about the input and output of the velocity controller + // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) + _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), local_pos_sp.thrust); + // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.