From 5027c68c0a5dd94c65c646fb7aa2117a8f7ae043 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 9 Mar 2020 10:04:04 +0100 Subject: [PATCH] mc_pos_control: handle takeoff and landing thrust limit the same way The landing thrust limit was after the position controller and could be inconsistent with what the takeoff limit did. This resulted in different thrust values sequentially getting applied during landing. --- .../mc_pos_control/mc_pos_control_main.cpp | 31 +++++-------------- 1 file changed, 8 insertions(+), 23 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 309463e6fd..4b5d0ef05d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -608,7 +608,11 @@ MulticopterPositionControl::Run() constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); - if (_takeoff.getTakeoffState() < TakeoffState::rampup) { + const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup; + const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight; + const bool flying_but_ground_contact = flying && _vehicle_land_detected.ground_contact; + + if (not_taken_off || flying_but_ground_contact) { // we are not flying yet and need to avoid any corrections reset_setpoint_to_nan(setpoint); setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; @@ -618,6 +622,9 @@ MulticopterPositionControl::Run() setpoint.yawspeed = 0.f; // prevent any integrator windup _control.resetIntegral(); + } + + if (not_taken_off) { // reactivate the task which will reset the setpoint to current state _flight_tasks.reActivate(); } @@ -661,13 +668,6 @@ MulticopterPositionControl::Run() 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) { - limit_thrust_during_landing(attitude_setpoint); - } - // publish attitude setpoint // It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized. // Not publishing when not running a flight task @@ -908,21 +908,6 @@ MulticopterPositionControl::start_flight_task() } } -void -MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint) -{ - if (_vehicle_land_detected.ground_contact - || _vehicle_land_detected.maybe_landed) { - // 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.resetIntegral(); - } -} - void MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force, bool warn)