From e7aa8997469857d7f7decf8163a79c85a7794ba7 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Fri, 14 Feb 2020 14:47:15 +0100 Subject: [PATCH] AutoLineSmoothVel: don't use _velocity_setpoint for constraints Unfortunately the constraints cannot be implemented using the _constraints because the controller uses that directly. This causes discontinuities in the velocity and/or accel at various points of the flight. In particular this was used in Takeoff. Instead this was done by changing target accel/velocity in the jerk-optimal velocity planner for Z. --- .../FlightTaskAutoLineSmoothVel.cpp | 33 +++++++++++++------ .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 4 +-- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 4 ++- 3 files changed, 27 insertions(+), 14 deletions(-) diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 6e17d8ee38..c5ff40fc65 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -237,8 +237,9 @@ float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const void FlightTaskAutoLineSmoothVel::_prepareSetpoints() { - // Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified - // that one is used as a velocity limit. + // Interface: A valid position setpoint generates a velocity target using conservative motion constraints. + // If a velocity is specified, that is used as a feedforward to track the position setpoint + // (ie. it assumes the position setpoint is moving at the specified velocity) // If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here. _want_takeoff = false; @@ -266,9 +267,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() math::trajectory::clampToZNorm(vel_sp_constrained, z_speed); for (int i = 0; i < 3; i++) { - // If available, constrain the velocity using _velocity_setpoint(.) + // If available, use the existing velocity as a feedforward, otherwise replace it if (PX4_ISFINITE(_velocity_setpoint(i))) { - _velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained(i), _velocity_setpoint(i)); + _velocity_setpoint(i) += vel_sp_constrained(i); } else { _velocity_setpoint(i) = vel_sp_constrained(i); @@ -287,9 +288,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed(); for (int i = 0; i < 2; i++) { - // If available, constrain the velocity using _velocity_setpoint(.) + // If available, use the existing velocity as a feedforward, otherwise replace it if (PX4_ISFINITE(_velocity_setpoint(i))) { - _velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained_xy(i), _velocity_setpoint(i)); + _velocity_setpoint(i) += vel_sp_constrained_xy(i); } else { _velocity_setpoint(i) = vel_sp_constrained_xy(i); @@ -303,9 +304,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() const float z_dir = math::sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition()); const float vel_sp_z = z_dir * _getMaxZSpeed(); - // If available, constrain the velocity using _velocity_setpoint(.) + // If available, use the existing velocity as a feedforward, otherwise replace it if (PX4_ISFINITE(_velocity_setpoint(2))) { - _velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2)); + _velocity_setpoint(2) += vel_sp_z; } else { _velocity_setpoint(2) = vel_sp_z; @@ -328,8 +329,20 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints() _trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get()); if (_velocity_setpoint(2) < 0.f) { // up - _trajectory[2].setMaxAccel(_param_mpc_acc_up_max.get()); - _trajectory[2].setMaxVel(_param_mpc_z_vel_max_up.get()); + float z_accel_constraint = _param_mpc_acc_up_max.get(); + float z_vel_constraint = _param_mpc_z_vel_max_up.get(); + + // The constraints are broken because they are used as hard limits by the position controller, so put this here + // until the constraints don't do things like cause controller integrators to saturate. Once the controller + // doesn't use z speed constraints, this can go in AutoMapper::_prepareTakeoffSetpoints(). Accel limit is to + // emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint. + 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()); + } + + _trajectory[2].setMaxVel(z_vel_constraint); + _trajectory[2].setMaxAccel(z_accel_constraint); } else { // down _trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get()); diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 449a84be3c..ea7b4c9fcb 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -142,9 +142,7 @@ void FlightTaskAutoMapper::_prepareTakeoffSetpoints() { // Takeoff is completely defined by target position _position_setpoint = _target; - const float speed_tko = (_dist_to_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up : - _param_mpc_tko_speed.get(); - _velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed + _velocity_setpoint = Vector3f(NAN, NAN, NAN); _gear.landing_gear = landing_gear_s::GEAR_DOWN; } diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp index bbd5d729e4..5f82bd3a80 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -70,7 +70,9 @@ protected: _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed (ParamFloat) _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed - (ParamFloat) _param_mpc_tko_speed + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) + _param_mpc_tko_ramp_t // time constant for smooth takeoff ramp ); private: