From 4627d5d6e45778971f477b687d6d15f36b263f13 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 14 Sep 2018 10:53:45 +0200 Subject: [PATCH] mc_pos_control_main smooth takeoff: enable position-xy/yaw control once above 0.5m from takeoff reference --- .../mc_pos_control/mc_pos_control_main.cpp | 38 +++++++++++++------ 1 file changed, 26 insertions(+), 12 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 7729d282a5..18aef49080 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -676,21 +676,35 @@ MulticopterPositionControl::run() update_smooth_takeoff(setpoint.z, setpoint.vz); } + // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards if (_in_smooth_takeoff) { + // during smooth takeoff, constrain speed to takeoff speed constraints.speed_up = _takeoff_speed; - // disable yaw command - setpoint.yaw = setpoint.yawspeed = NAN; - // don't control position in xy - setpoint.x = setpoint.y = NAN; - setpoint.vx = setpoint.vy = NAN; - setpoint.thrust[0] = setpoint.thrust[1] = NAN; - - if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) { - setpoint.vx = setpoint.vy = 0.0f; // try to keep zero velocity - - } else { - setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; // just keeping pointing upwards + // altitude above reference takeoff + const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); + // altitude threshold at which full control is enabled + const float alt_threshold = 0.5f; //at 0.5m above grouund, control all setpoints + + // disable position-xy / yaw control when close to ground + if (alt_above_tko <= alt_threshold) { + // don't control position in xy + setpoint.x = setpoint.y = NAN; + setpoint.vx = setpoint.vy = NAN; + setpoint.thrust[0] = setpoint.thrust[1] = NAN; + setpoint.yaw = setpoint.yawspeed = NAN; + + if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) { + setpoint.vx = setpoint.vy = 0.0f; // try to keep zero velocity + + } else { + setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; // just keeping pointing upwards + } + + // if there is a valid yaw estimate, just set setpoint to yaw + if (PX4_ISFINITE(_states.yaw)) { + setpoint.yaw = _states.yaw; + } } }