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.
This commit is contained in:
Julian Kent
2020-02-18 13:59:58 +01:00
committed by Julian Kent
parent b365fbf1e9
commit e7aa899746
3 changed files with 27 additions and 14 deletions
@@ -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;
}
@@ -70,7 +70,9 @@ protected:
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
);
private: