Auto traj - Add Trajectory logging

- move the update after the integration: a new computed jerk has an impact at the next epoch only
- add jerk reduction in case of too large integration time: when a jerk of "min_jerk" during dt is too much
- add jerk reduction if the integration time is larger than the predicted one and that integrating that jerk would lead to an acceleration overshoot
- rename some variables

msg/vehicle_local_position_setpoint.msg

@@ -14,4 +14,9 @@ float32 vz		# in meters/sec
 float32 acc_x		# in meters/(sec*sec)
 float32 acc_y		# in meters/(sec*sec)
 float32 acc_z		# in meters/(sec*sec)
+float32 jerk_x		# in meters/(sec*sec*sec)
+float32 jerk_y		# in meters/(sec*sec*sec)
+float32 jerk_z		# in meters/(sec*sec*sec)
 float32[3] thrust	# normalized thrust vector in NED
+
+# TOPICS vehicle_local_position_setpoint trajectory_setpoint

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -49,6 +49,8 @@ bool FlightTaskAutoLineSmoothVel::activate()
 		_trajectory[i].reset(0.f, _velocity(i), _position(i));
 	}
 
+	_updateTrajConstraints();
+
 	return ret;
 }
 
@@ -121,15 +123,15 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 
 		float speed_sp_track = Vector2f(pos_traj_to_dest).length() * MPC_XY_TRAJ_P.get();
 		speed_sp_track = math::constrain(speed_sp_track, 0.0f, MPC_XY_CRUISE.get());
-		Vector2f velocity_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
+		Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
 
 		for (int i = 0; i < 2; i++) {
 			// If available, constrain the velocity using _velocity_setpoint(.)
 			if (PX4_ISFINITE(_velocity_setpoint(i))) {
-				_velocity_setpoint(i) = constrain_one_side(velocity_sp_xy(i), _velocity_setpoint(i));
+				_velocity_setpoint(i) = constrain_one_side(vel_sp_xy(i), _velocity_setpoint(i));
 
 			} else {
-				_velocity_setpoint(i) = velocity_sp_xy(i);
+				_velocity_setpoint(i) = vel_sp_xy(i);
 			}
 
 			_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
@@ -145,15 +147,15 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	}
 
 	if (PX4_ISFINITE(_position_setpoint(2))) {
-		const float velocity_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
+		const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
 				       MPC_Z_TRAJ_P.get(); // Generate a velocity target for the trajectory using a simple P loop
 
 		// If available, constrain the velocity using _velocity_setpoint(.)
 		if (PX4_ISFINITE(_velocity_setpoint(2))) {
-			_velocity_setpoint(2) = constrain_one_side(velocity_sp_z, _velocity_setpoint(2));
+			_velocity_setpoint(2) = constrain_one_side(vel_sp_z, _velocity_setpoint(2));
 
 		} else {
-			_velocity_setpoint(2) = velocity_sp_z;
+			_velocity_setpoint(2) = vel_sp_z;
 		}
 
 	} else if (!PX4_ISFINITE(_velocity_setpoint(2))) {
@@ -162,7 +164,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	}
 }
 
-void FlightTaskAutoLineSmoothVel::_generateTrajectory()
+void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
 {
 	// Update the constraints of the trajectories
 	_trajectory[0].setMaxAccel(MPC_ACC_HOR_MAX.get()); // TODO : Should be computed using heading
@@ -181,13 +183,10 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 		_trajectory[2].setMaxAccel(MPC_ACC_DOWN_MAX.get());
 		_trajectory[2].setMaxVel(MPC_Z_VEL_MAX_DN.get());
 	}
+}
 
-	for (int i = 0; i < 3; ++i) {
-		_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
-	}
-
-	VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
-
+void FlightTaskAutoLineSmoothVel::_generateTrajectory()
+{
 	/* Slow down the trajectory by decreasing the integration time based on the position error.
 	 * This is only performed when the drone is behind the trajectory
 	 */
@@ -204,9 +203,26 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 		time_stretch = 1.f;
 	}
 
-	Vector3f accel_sp_smooth; // Dummy variable
+	Vector3f jerk_sp_smooth;
+	Vector3f accel_sp_smooth;
+	Vector3f vel_sp_smooth;
+	Vector3f pos_sp_smooth;
 
 	for (int i = 0; i < 3; ++i) {
-		_trajectory[i].integrate(_deltatime * time_stretch, accel_sp_smooth(i), _velocity_setpoint(i), _position_setpoint(i));
+		_trajectory[i].integrate(_deltatime, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i));
+		jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
 	}
+
+	_updateTrajConstraints();
+
+	for (int i = 0; i < 3; ++i) {
+		_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
+	}
+
+	VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
+
+	_jerk_setpoint = jerk_sp_smooth;
+	_acceleration_setpoint = accel_sp_smooth;
+	_velocity_setpoint = vel_sp_smooth;
+	_position_setpoint = pos_sp_smooth;
 }

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -70,6 +70,7 @@ protected:
 
 	inline float constrain_one_side(float val, float constrain);
 	void _generateHeadingAlongTrack(); /**< Generates heading along track. */
+	void _updateTrajConstraints();
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _generateTrajectory();
 	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions

src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp

@@ -4,7 +4,7 @@
 
 constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
-const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
+const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
 
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, vehicle_constraints_s::GEAR_KEEP, {}};
 const vehicle_trajectory_waypoint_s FlightTask::empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
@@ -71,6 +71,10 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 	vehicle_local_position_setpoint.acc_y = _acceleration_setpoint(1);
 	vehicle_local_position_setpoint.acc_z = _acceleration_setpoint(2);
 
+	vehicle_local_position_setpoint.jerk_x = _jerk_setpoint(0);
+	vehicle_local_position_setpoint.jerk_y = _jerk_setpoint(1);
+	vehicle_local_position_setpoint.jerk_z = _jerk_setpoint(2);
+
 	_thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
 	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
 	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
@@ -83,6 +87,7 @@ void FlightTask::_resetSetpoints()
 	_position_setpoint.setAll(NAN);
 	_velocity_setpoint.setAll(NAN);
 	_acceleration_setpoint.setAll(NAN);
+	_jerk_setpoint.setAll(NAN);
 	_thrust_setpoint.setAll(NAN);
 	_yaw_setpoint = _yawspeed_setpoint = NAN;
 	_desired_waypoint = FlightTask::empty_trajectory_waypoint;

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -196,11 +196,12 @@ protected:
 	 * If more than one type of setpoint is set, then order of control is a as follow: position, velocity,
 	 * acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the
 	 * _velocity_setpoint is used as feedforward.
-	 * _acceleration_setpoint is currently not supported.
+	 * _acceleration_setpoint and _jerk_setpoint are currently not supported.
 	 */
 	matrix::Vector3f _position_setpoint;
 	matrix::Vector3f _velocity_setpoint;
 	matrix::Vector3f _acceleration_setpoint;
+	matrix::Vector3f _jerk_setpoint;
 	matrix::Vector3f _thrust_setpoint;
 	float _yaw_setpoint;
 	float _yawspeed_setpoint;

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp

@@ -52,6 +52,31 @@ void VelocitySmoothing::reset(float accel, float vel, float pos)
 	_pos = pos;
 }
 
+float VelocitySmoothing::saturateT1ForAccel(float accel_prev, float max_jerk, float T1)
+{
+	/* Check maximum acceleration, saturate and recompute T1 if needed */
+	float accel_T1 = accel_prev + max_jerk * T1;
+	float T1_new = T1;
+
+	if (accel_T1 > _max_accel) {
+		T1_new = (_max_accel - accel_prev) / max_jerk;
+
+	} else if (accel_T1 < -_max_accel) {
+		T1_new = (-_max_accel - accel_prev) / max_jerk;
+	}
+
+	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;
@@ -59,6 +84,7 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
 	float delta = b * b - 4.f * c;
 
 	if (delta < 0.f) {
+		// Solution is not real
 		return 0.f;
 	}
 
@@ -68,20 +94,12 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
 
 	float T1 = math::max(math::max(T1_plus, T1_minus), 0.f);
 
-//	if (T1 < FLT_EPSILON) {
-//		// debug
-//		printf("No feasible solution found, set T1 = 0\n");
-//		printf("T1_plus = %.3f T1_minus = %.3f\n", (double) T1_plus, (double) T1_minus);
-//	}
+	T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
 
-	/* Check maximum acceleration, saturate and recompute T1 if needed */
-	float a1 = accel_prev + max_jerk * T1;
-
-	if (a1 > _max_accel) {
-		T1 = (_max_accel - accel_prev) / max_jerk;
-
-	} else if (a1 < -_max_accel) {
-		T1 = (-_max_accel - accel_prev) / max_jerk;
+	if ((T1 > FLT_EPSILON) &&
+	    (T1 < _dt)) {
+		_max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1);
+		T1 = _dt;
 	}
 
 	return math::max(T1, 0.f);
@@ -114,6 +132,14 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
 		T1 = T1_plus;
 	}
 
+	T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
+
+	if ((T1 > FLT_EPSILON) &&
+	    (T1 < _dt)) {
+		_max_jerk_T1 = recomputeMaxJerk(accel_prev, max_jerk, T1);
+		T1 = _dt;
+	}
+
 	return T1;
 }
 
@@ -144,6 +170,7 @@ 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;
 
@@ -153,6 +180,7 @@ void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float
 
 	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;
 
@@ -217,30 +245,41 @@ void VelocitySmoothing::updateDurations(float T123)
 void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
 				  float &pos_setpoint_smooth)
 {
-	integrate(_dt, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
+	integrate(_dt, 1.f, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
 }
 
-void VelocitySmoothing::integrate(float dt, float &accel_setpoint_smooth, float &vel_setpoint_smooth,
+void VelocitySmoothing::integrate(float dt, float integration_scale_factor, float &accel_setpoint_smooth,
+				  float &vel_setpoint_smooth,
 				  float &pos_setpoint_smooth)
 {
-	/* Integrate the trajectory */
-	float accel_new, vel_new, pos_new;
-	integrateT(dt, _jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
-
 	/* Apply correct jerk (min, max or zero) */
-	if (_T1 > 0.f) {
+	if (_T1 > FLT_EPSILON) {
 		_jerk = _max_jerk_T1;
 
-	} else if (_T2 > 0.f) {
+		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.
+			_jerk *= _dt / dt; // Keep the same area _dt * _jerk = dt * jerk_new
+		}
+
+	} else if (_T2 > FLT_EPSILON) {
 		_jerk = 0.f;
 
-	} else if (_T3 > 0.f) {
+	} else if (_T3 > FLT_EPSILON) {
 		_jerk = -_max_jerk_T1;
 
+		if (_T3 < dt) {
+			// Same as for _T1 < dt above
+			_jerk *= _dt / dt;
+		}
+
 	} else {
 		_jerk = 0.f;
 	}
 
+	/* Integrate the trajectory */
+	float accel_new, vel_new, pos_new;
+	integrateT(dt * integration_scale_factor, _jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
+
 	_accel = accel_new;
 	_vel = vel_new;
 	_pos = pos_new;

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp

@@ -84,7 +84,8 @@ public:
 	 * @param pos_setpoint_smooth returned smoothed position setpoint
 	 */
 	void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
-	void integrate(float dt, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
+	void integrate(float dt, float integration_scale_factor, float &accel_setpoint_smooth, float &vel_setpoint_smooth,
+		       float &pos_setpoint_smooth);
 
 	/* Get / Set constraints (constraints can be updated at any time) */
 	float getMaxJerk() const { return _max_jerk; }
@@ -96,6 +97,9 @@ public:
 	float getMaxVel() const { return _max_vel; }
 	void setMaxVel(float max_vel) { _max_vel = max_vel; }
 
+	float getCurrentJerk() const { return _jerk; }
+	void setCurrentAcceleration(const float accel) { _accel = accel; }
+	float getCurrentAcceleration() const { return _accel; }
 	void setCurrentVelocity(const float vel) { _vel = vel; }
 	float getCurrentVelocity() const { return _vel; }
 	void setCurrentPosition(const float pos) { _pos = pos; }
@@ -129,6 +133,8 @@ private:
 	 * Compute increasing acceleration time using total time constraint
 	 */
 	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
 	 */
@@ -150,7 +156,7 @@ private:
 
 	/* Inputs */
 	float _vel_sp;
-	float _dt = 0.f;
+	float _dt = 1.f;
 
 	/* Constraints */
 	float _max_jerk = 22.f;
@@ -158,10 +164,10 @@ private:
 	float _max_vel = 6.f;
 
 	/* State (previous setpoints) */
-	float _jerk;
-	float _accel;
-	float _vel;
-	float _pos;
+	float _jerk = 0.f;
+	float _accel = 0.f;
+	float _vel = 0.f;
+	float _pos = 0.f;
 
 	float _max_jerk_T1 = 0.f; ///< jerk during phase T1 (with correct sign)
 

src/modules/logger/logger.cpp

@@ -641,6 +641,7 @@ void Logger::add_default_topics()
 	add_topic("sensor_preflight", 200);
 	add_topic("system_power", 500);
 	add_topic("tecs_status", 200);
+	add_topic("trajectory_setpoint");
 	add_topic("telemetry_status");
 	add_topic("vehicle_air_data", 200);
 	add_topic("vehicle_attitude", 30);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -100,6 +100,7 @@ private:
 	bool 		_in_smooth_takeoff = false; 		/**<true if takeoff ramp is applied */
 
 	orb_advert_t	_att_sp_pub{nullptr};			/**< attitude setpoint publication */
+	orb_advert_t	_traj_sp_pub{nullptr};		/**< trajectory setpoints publication */
 	orb_advert_t	_local_pos_sp_pub{nullptr};		/**< vehicle local position setpoint publication */
 	orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
 	orb_advert_t _pub_vehicle_command{nullptr};           /**< vehicle command publication */
@@ -218,6 +219,12 @@ private:
 	 */
 	void publish_local_pos_sp(const vehicle_local_position_setpoint_s &local_pos_sp);
 
+	/**
+	 * Publish local position setpoint.
+	 * This is only required for logging.
+	 */
+	void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj);
+
 	/**
 	 * Checks if smooth takeoff is initiated.
 	 * @param position_setpoint_z the position setpoint in the z-Direction
@@ -751,6 +758,8 @@ MulticopterPositionControl::run()
 				limit_thrust_during_landing(thr_sp);
 			}
 
+			publish_trajectory_sp(setpoint);
+
 			// Fill local position, velocity and thrust setpoint.
 			vehicle_local_position_setpoint_s local_pos_sp{};
 			local_pos_sp.timestamp = hrt_absolute_time();
@@ -1204,6 +1213,18 @@ MulticopterPositionControl::publish_attitude()
 	}
 }
 
+void
+MulticopterPositionControl::publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj)
+{
+	// publish trajectory
+	if (_traj_sp_pub != nullptr) {
+		orb_publish(ORB_ID(trajectory_setpoint), _traj_sp_pub, &traj);
+
+	} else {
+		_traj_sp_pub = orb_advertise(ORB_ID(trajectory_setpoint), &traj);
+	}
+}
+
 void
 MulticopterPositionControl::publish_local_pos_sp(const vehicle_local_position_setpoint_s &local_pos_sp)
 {