fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 19:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

AutoLineSmoothVel: don't use _velocity_setpoint for constraints

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+23 -10
View File
@@ -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());
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+1 -3
View File
@@ -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;
}
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
+3 -1
View File
@@ -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: