diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg
index 3932edb72a8959ab2993031f6cc2b59617652f5f..dc60f203841adf3a23cb0224ae620c6486a1ce91 100644
--- a/msg/vehicle_local_position_setpoint.msg
+++ b/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
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 3bb4ab4719b4c2193fed636a46ebe42b70776492..7099aff6268236bf65a838691b24a4b361e54660 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/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;
}
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
index cea5b5560cc6c6902d2aa251e9a68ab48c80c444..aaa4e1f05d37b809d640770ac7926ad1f328c732 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+++ b/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
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
index b314ae0b9507272f0375e0d1beed088a78cc3b53..1693c4ab003b3cffc61c20435ea0120d8382d501 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+++ b/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;
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index aa3e68951b6ccda9128beb2a84901e90d46ea038..759a0b70d3b393f628a9d4346f5f65dc0c10caa5 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/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;
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index b3e2e329a37226331bd56fcf9b52ecf73432da7e..d65ec361532385aa9eb2c19f99e17c682424a9ab 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/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);
-// }
-
- /* 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;
+ T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
- } 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;
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index ec035d4924896fcd5876bfba9fd70a4f66ef1a52..aada8e285241f3d848372406fb45029d11a2767f 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/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)
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f3240c6bf073109fec18398e2d5ad51aaf904cd8..a5f4c93e5199f39ab408f9f9e1219ec8d1c4464a 100644
--- a/src/modules/logger/logger.cpp
+++ b/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);
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 65c30f1338958ce8569be38a7b9f1c36d541003e..3118f2b9dfbdac8a5700ecdf9b305b896baf9764 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/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)
{