From c6cc9f09023c7bc78d85f08b716b460d6d6acdf7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 4 Nov 2019 18:26:08 +0100 Subject: [PATCH] mc_pos_control: fix cutting thrust when landed by applying it directly to the attitude setpoint which is the output of the position controller. The problem was that before the input to the attitude setpoint generation was adjusted to generate a level attitude with zero thrust keeping the heading. I refactored the PositionControl class in #13262 to directly generate the attitude setpoint output. So here I'm adjusting the attitude setpoint to do the exact same thing as before but without interleaving with the PositionControl logic. --- .../mc_pos_control/mc_pos_control_main.cpp | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) 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 81319b368a..715658fe7c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -234,7 +234,7 @@ private: * Thrust is adjusted to support the land-detector during detection. * @param setpoint gets adjusted based on land-detector state */ - void limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint); + void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint); /** * Start flightasks based on navigation state. @@ -669,17 +669,16 @@ MulticopterPositionControl::Run() // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), Vector3f(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. - if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { - limit_thrust_during_landing(local_pos_sp); - } - vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; _control.getAttitudeSetpoint(attitude_setpoint); + // 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. + if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { + limit_thrust_during_landing(attitude_setpoint); + } // publish attitude setpoint // Note: this requires review. The reason for not sending @@ -928,16 +927,16 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint) +MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint) { if (_vehicle_land_detected.ground_contact || _vehicle_land_detected.maybe_landed) { - // we set thrust to zero, this will help to decide if we are actually landed or not - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; - // set yaw-sp to current yaw to avoid any corrections - setpoint.yaw = _states.yaw; - setpoint.yawspeed = 0.f; - // prevent any integrator windup + // we set the collective thrust to zero, this will help to decide if we are actually landed or not + setpoint.thrust_body[2] = 0.f; + // go level to avoid corrections but keep the heading we have + Quatf(AxisAngle(0, 0, _states.yaw)).copyTo(setpoint.q_d); + setpoint.yaw_sp_move_rate = 0.f; + // prevent any position control integrator windup _control.resetIntegralXY(); _control.resetIntegralZ(); }