diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 5e0109ee5f2f..b0ffe8378b15 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -76,8 +76,10 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes) } _position_lock_xy_active = false; + _position_lock_z_active = false; _position_setpoint_xy_locked(0) = NAN; _position_setpoint_xy_locked(1) = NAN; + _position_setpoint_z_locked = NAN; } void FlightTaskManualPositionSmoothVel::_updateSetpoints() @@ -100,25 +102,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _smoothing[2].setMaxVel(_constraints.speed_down); } - Vector2f vel_xy_sp = Vector2f(&_velocity_setpoint(0)); float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()}; - float jerk_xy = _jerk_max.get(); - - if (_jerk_min.get() > _jerk_max.get()) { - _jerk_min.set(0.f); - } - - if (_jerk_min.get() > FLT_EPSILON) { - if (vel_xy_sp.length() < FLT_EPSILON) { // Brake - jerk_xy = _jerk_max.get(); - - } else { - jerk_xy = _jerk_min.get(); - } - } - - jerk[0] = jerk_xy; - jerk[1] = jerk_xy; /* Check for position unlock * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint @@ -140,6 +124,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _position_lock_xy_active = false; } + if (fabsf(_sticks_expo(2)) > FLT_EPSILON) { + if (_position_lock_z_active) { + _smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback( + 2)); // Start the trajectory at the current velocity setpoint + _position_setpoint_z_locked = NAN; + } + + _position_lock_z_active = false; + } + for (int i = 0; i < 3; ++i) { _smoothing[i].setMaxJerk(jerk[i]); _smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i)); @@ -157,27 +151,41 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() } } + if (!_position_lock_xy_active) { + _smoothing[0].setCurrentPosition(_position(0)); + _smoothing[1].setCurrentPosition(_position(1)); + } + + if (!_position_lock_z_active) { + _smoothing[2].setCurrentPosition(_position(2)); + } + Vector3f pos_sp_smooth; - Vector3f accel_sp_smooth; for (int i = 0; i < 3; ++i) { - if (!_position_lock_xy_active) { - _smoothing[i].setCurrentPosition(_position(i)); - } - _smoothing[i].integrate(accel_sp_smooth(i), _vel_sp_smooth(i), pos_sp_smooth(i)); + _smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i)); _velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward + _jerk_setpoint(i) = _smoothing[i].getCurrentJerk(); } // Check for position lock transition if (Vector2f(_vel_sp_smooth).length() < 0.01f && - Vector2f(accel_sp_smooth).length() < .2f && + Vector2f(_acceleration_setpoint).length() < .2f && sticks_expo_xy.length() <= FLT_EPSILON) { _position_setpoint_xy_locked(0) = pos_sp_smooth(0); _position_setpoint_xy_locked(1) = pos_sp_smooth(1); _position_lock_xy_active = true; } + if (fabsf(_vel_sp_smooth(2)) < 0.01f && + fabsf(_acceleration_setpoint(2)) < .2f && + fabsf(_sticks_expo(2)) <= FLT_EPSILON) { + _position_setpoint_z_locked = pos_sp_smooth(2); + _position_lock_z_active = true; + } + _position_setpoint(0) = _position_setpoint_xy_locked(0); _position_setpoint(1) = _position_setpoint_xy_locked(1); + _position_setpoint(2) = _position_setpoint_z_locked; } diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index 945c76fe4504..cd8e8e9f182a 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -69,6 +69,9 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions matrix::Vector3f _vel_sp_smooth; bool _position_lock_xy_active{false}; + bool _position_lock_z_active{false}; matrix::Vector2f _position_setpoint_xy_locked; + float _position_setpoint_z_locked; + uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ }; diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index 5503d4bbfbdc..9f25fdfe15c0 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -69,15 +69,6 @@ float VelocitySmoothing::saturateT1ForAccel(float accel_prev, float max_jerk, fl return T1_new; } -float VelocitySmoothing::recomputeMaxJerk(float accel_prev, float max_jerk, float T1) -{ - /* If T1 is smaller than dt, it means that the jerk is too large to reach the - * desired acceleration with a bang-bang signal => recompute the maximum jerk - */ - float accel_T1 = accel_prev + max_jerk * T1; - return (accel_T1 - accel_prev) / T1; -} - float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk) { float b = 2.f * accel_prev / max_jerk; @@ -93,14 +84,22 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s float T1_plus = (-b + sqrt_delta) * 0.5f; float T1_minus = (-b - sqrt_delta) * 0.5f; - float T1 = math::max(math::max(T1_plus, T1_minus), 0.f); + float T3_plus = accel_prev / max_jerk + T1_plus; + float T3_minus = accel_prev / max_jerk + T1_minus; + + float T1 = 0.f; + + if (T1_plus >= 0.f && T3_plus >= 0.f) { + T1 = T1_plus; + + } else if (T1_minus >= 0.f && T3_minus >= 0.f) { + T1 = T1_minus; + } T1 = saturateT1ForAccel(accel_prev, max_jerk, T1); - if ((T1 > FLT_EPSILON) && - (T1 < _dt)) { - _max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1); - T1 = _dt; + if (T1 < _dt) { + T1 = 0.f; } return math::max(T1, 0.f); @@ -135,10 +134,8 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev, T1 = saturateT1ForAccel(accel_prev, max_jerk, T1); - if ((T1 > FLT_EPSILON) && - (T1 < _dt)) { - _max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1); - T1 = _dt; + if (T1 < _dt) { + T1 = 0.f; } return T1; @@ -151,6 +148,11 @@ float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float v float f = accel_prev * T1 + max_jerk * T1 * T1 * 0.5f + vel_prev + accel_prev * T3 + max_jerk * T1 * T3 - max_jerk * T3 * T3 * 0.5f; float T2 = (vel_setpoint - f) / (accel_prev + max_jerk * T1); + + if (T2 < _dt) { + T2 = 0.f; + } + return math::max(T2, 0.f); } @@ -163,6 +165,12 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3) float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk) { float T3 = accel_prev / max_jerk + T1; + + if (T1 < FLT_EPSILON && T3 < _dt && T3 > 0.f) { + T3 = _dt; + _max_jerk_T1 = accel_prev / T3; + } + return math::max(T3, 0.f); } @@ -171,31 +179,15 @@ void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float { accel_out = jerk * dt + accel_prev; - // Paranoid check, should never be outside the saturations - if (accel_out > _max_accel) { - accel_out = _max_accel; - - } else if (accel_out < -_max_accel) { - accel_out = -_max_accel; - } - vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev; - // Paranoid check, should never be outside the saturations - if (vel_out > _max_vel) { - vel_out = _max_vel; - - } else if (vel_out < -_max_vel) { - vel_out = -_max_vel; - } - pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos; } void VelocitySmoothing::updateDurations(float dt, float vel_setpoint) { - _vel_sp = vel_setpoint; - _dt = dt; + _vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel); + _dt = math::max(dt, FLT_EPSILON); updateDurations(); } @@ -214,18 +206,9 @@ void VelocitySmoothing::updateDurations(float T123) T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1); } - /* Force T1/2/3 to zero if smaller than an epoch to avoid chattering */ - if (T1 < _dt) { - T1 = 0.f; - } - // compute decreasing acceleration time T3 = computeT3(T1, _accel, _max_jerk_T1); - if (T3 < _dt) { - T3 = 0.f; - } - // compute constant acceleration time if (PX4_ISFINITE(T123)) { T2 = computeT2(T123, T1, T3); @@ -234,10 +217,6 @@ void VelocitySmoothing::updateDurations(float T123) T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1); } - if (T2 < _dt) { - T2 = 0.f; - } - _T1 = T1; _T2 = T2; _T3 = T3; @@ -257,8 +236,8 @@ void VelocitySmoothing::integrate(float dt, float integration_scale_factor, floa if (_T1 > FLT_EPSILON) { _jerk = _max_jerk_T1; - if (_T1 < dt) { - // _T1 was supposed to be _dt, however, now, dt is bogger than _dt. We have to reduce the jerk to avoid an acceleration overshoot. + if (_T1 < dt && dt > _dt) { + // _T1 was supposed to be _dt, however, now, dt is bigger than _dt. We have to reduce the jerk to avoid an acceleration overshoot. _jerk *= _dt / dt; // Keep the same area _dt * _jerk = dt * jerk_new } @@ -268,7 +247,7 @@ void VelocitySmoothing::integrate(float dt, float integration_scale_factor, floa } else if (_T3 > FLT_EPSILON) { _jerk = -_max_jerk_T1; - if (_T3 < dt) { + if (_T3 < dt && dt > _dt) { // Same as for _T1 < dt above _jerk *= _dt / dt; } diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp index aada8e285241..048e12c9d5a6 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp @@ -134,7 +134,6 @@ class VelocitySmoothing */ inline float computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk); inline float saturateT1ForAccel(float accel_prev, float max_jerk, float T1); - inline float recomputeMaxJerk(float accel_prev, float max_jerk, float T1); /** * Compute constant acceleration time */ diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index c1c4bc62dc13..cc18232ac7c0 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -221,7 +221,7 @@ void PositionControl::_positionController() // Constrain horizontal velocity by prioritizing the velocity component along the // the desired position setpoint over the feed-forward term. const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position), - Vector2f(_vel_sp - vel_sp_position), _constraints.speed_xy); + Vector2f(_vel_sp - vel_sp_position), MPC_XY_VEL_MAX.get()); _vel_sp(0) = vel_sp_xy(0); _vel_sp(1) = vel_sp_xy(1); // Constrain velocity in z-direction. 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 bfd9ae3fa895..237f3d53bd44 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -686,6 +686,8 @@ MulticopterPositionControl::run() } } + publish_trajectory_sp(setpoint); + /* desired waypoints for obstacle avoidance: * point_0 contains the current position with the desired velocity * point_1 contains _pos_sp_triplet.current if valid @@ -758,7 +760,6 @@ MulticopterPositionControl::run() // Generate desired thrust and yaw. _control.generateThrustYawSetpoint(_dt); - publish_trajectory_sp(setpoint); // Fill local position, velocity and thrust setpoint. // This message contains setpoints where each type of setpoint is either the input to the PositionController @@ -1045,18 +1046,15 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl 0.2f; // takeoff was initiated through velocity setpoint - _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < math::min(-_tko_speed.get(), -0.6f); + _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { // There is a position setpoint above current position or velocity setpoint larger than // takeoff speed. Enable smooth takeoff. _in_smooth_takeoff = true; - _takeoff_speed = -0.5f; + _takeoff_speed = 0.f; _takeoff_reference_z = _states.position(2); - } else { - // Default - _in_smooth_takeoff = false; } } } @@ -1076,19 +1074,24 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float } // Ramp up takeoff speed. - _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); - _takeoff_speed = math::min(_takeoff_speed, desired_tko_speed); + if (_takeoff_ramp_time.get() > _dt) { + _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); + + } else { + // No ramp, directly go to desired takeoff speed + _takeoff_speed = desired_tko_speed; + } + + _takeoff_speed = math::min(_takeoff_speed, _tko_speed.get()); // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. if (!_smooth_velocity_takeoff) { _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get()); - } else { - _in_smooth_takeoff = _takeoff_speed < -vz_sp; + } else { + // Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector + _in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed; } - - } else { - _in_smooth_takeoff = false; } } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index f87debef19a2..2d5b80a410c2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -650,8 +650,9 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); * * Increasing this value will make automatic and manual takeoff slower. * If it's too slow the drone might scratch the ground and tip over. + * A time constant of 0 disables the ramp * - * @min 0.1 + * @min 0 * @max 1 * @group Multicopter Position Control */