Dev traj fixes

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;
 }

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 */
 };

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;
 		}

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
 	 */

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.

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;
 	}
 }
 

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
  */