From 0ad3ddd864642ba5db368cf9ad5cd3ac8c21eab6 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 7 Aug 2020 14:15:51 +0200 Subject: [PATCH] Tko ramp: fix takeoff issue with large tko ramp time Because the velocity starts with a positive value (down) to ramp-up slowly the motors, the position setpoint (integral) was going down into the ground, canceling the input of the velocity controller during the early takeoff phase. Instead of a smooth ramp, the behavior was a delay followed by a quick takeoff or a land detection. Setting the current altitude setpoint with the current altitude during this phase avoids the canceling effect and creates a takeoff purely velocity-based. --- .../tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index f411b40f68..6349c34452 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -326,6 +326,11 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints() if (_type == WaypointType::takeoff && _dist_to_ground < _param_mpc_land_alt1.get()) { z_vel_constraint = _param_mpc_tko_speed.get(); z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get()); + + // Keep the altitude setpoint at the current altitude + // to avoid having it going down into the ground during + // the initial ramp as the velocity does not start at 0 + _trajectory[2].setCurrentPosition(_position(2)); } _trajectory[2].setMaxVel(z_vel_constraint);