diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 5eb228aa7b..3a07d25ae3 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -136,6 +136,8 @@ public: */ void reActivate(); + void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); } + private: /** diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index d2d843e226..ca98239ef5 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -147,6 +147,8 @@ public: */ virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}; + void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } + protected: uORB::Subscription *_sub_vehicle_local_position{nullptr}; @@ -197,6 +199,9 @@ protected: float _yaw_setpoint; float _yawspeed_setpoint; + matrix::Vector3f _velocity_setpoint_feedback; + matrix::Vector3f _thrust_setpoint_feedback; + /** * Vehicle constraints. * The constraints can vary with tasks. diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index e18d28a9bc..5a2b215e88 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -47,6 +47,10 @@ bool FlightTaskManualPositionSmoothVel::activate() _smoothing[i].reset(0.f, _velocity(i), _position(i)); } + _position_lock_xy_active = false; + _position_setpoint_xy_locked(0) = NAN; + _position_setpoint_xy_locked(1) = NAN; + return ret; } @@ -81,9 +85,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() if (_jerk_min.get() > FLT_EPSILON) { if (vel_xy_sp.length() < FLT_EPSILON) { // Brake - jerk_xy = _jerk_min.get() + (_jerk_max.get() - _jerk_min.get()); - - } else if (vel_xy_sp.dot(vel_xy_sp_smooth) < -FLT_EPSILON) { // Reverse jerk_xy = _jerk_max.get(); } else { @@ -94,6 +95,23 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() 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 + * is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null + * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller + * is used to set current velocity of the trajectory. + */ + Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0)); + if (sticks_expo_xy.length() > FLT_EPSILON) { + if (_position_lock_xy_active) { + _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); // Start the trajectory at the current velocity setpoint + _smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); + _position_setpoint_xy_locked(0) = NAN; + _position_setpoint_xy_locked(1) = NAN; + } + _position_lock_xy_active = false; + } + for (int i = 0; i < 3; ++i) { _smoothing[i].setMaxJerk(jerk[i]); _smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i)); @@ -101,11 +119,27 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only + Vector3f pos_sp_smooth; + Vector3f accel_sp_smooth; + for (int i = 0; i < 3; ++i) { - float smoothed_position_setpoint; - _smoothing[i].integrate(_position(i), _vel_sp_smooth(i), smoothed_position_setpoint); - //printf("\nTraj %d\tNew total time = %.3f", i, (double)_smoothing[i].getTotalTime()); - _position_setpoint(i) = smoothed_position_setpoint; - _velocity_setpoint(i) = _vel_sp_smooth(i); + _smoothing[i].setCurrentPosition(_position(i)); + _smoothing[i].integrate(accel_sp_smooth(i), _vel_sp_smooth(i), pos_sp_smooth(i)); + _velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward } + + // Check for position lock transition + Vector2f accel_setpoint_xy_smooth = Vector2f(&accel_sp_smooth(0)); + if (vel_xy_sp_smooth.length() < 0.002f && + accel_setpoint_xy_smooth.length() < .2f && + sticks_expo_xy.length() <= FLT_EPSILON) { + if (!_position_lock_xy_active) { + _position_setpoint_xy_locked(0) = pos_sp_smooth(0); + _position_setpoint_xy_locked(1) = pos_sp_smooth(1); + } + _position_lock_xy_active = true; + } + + _position_setpoint(0) = _position_setpoint_xy_locked(0); + _position_setpoint(1) = _position_setpoint_xy_locked(1); } diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index 5b02552c36..b224ea65c5 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -65,4 +65,6 @@ private: VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions matrix::Vector3f _vel_sp_smooth; + bool _position_lock_xy_active{false}; + matrix::Vector2f _position_setpoint_xy_locked; }; diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index 7b0b384088..81d6018cd9 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -214,7 +214,7 @@ void VelocitySmoothing::updateDurations(float T123) _T3 = T3; } -void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth, +void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth) { /* Integrate the trajectory */ @@ -238,14 +238,8 @@ void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth, _accel = accel_new; _vel = vel_new; - /* Lock the position setpoint if the error is bigger than some value */ - float x_err = pos_new - pos; - - if (fabsf(x_err) <= max_pos_err) { - _pos = pos_new; - } // else: keep last position - /* set output variables */ + accel_setpoint_smooth = _accel; vel_setpoint_smooth = _vel; pos_setpoint_smooth = _pos; } diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp index 5a376f5122..9fbf777f5b 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp @@ -77,11 +77,11 @@ public: /** * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk - * @param pos Current vehicle's position (used for position error clamping) + * @param acc_setpoint_smooth returned smoothed acceleration setpoint * @param vel_setpoint_smooth returned smoothed velocity setpoint * @param pos_setpoint_smooth returned smoothed position setpoint */ - void integrate(float pos, float &vel_setpoint_smooth, float &pos_setpoint_smooth); + void integrate(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; } @@ -93,6 +93,9 @@ public: float getMaxVel() const { return _max_vel; } void setMaxVel(float max_vel) { _max_vel = max_vel; } + void setCurrentVelocity(const float vel) { _vel = vel; } + void setCurrentPosition(const float pos) { _pos = pos; } + /** * Synchronize several trajectories to have the same total time. This is required to generate * straight lines. 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 bc331fa25e..9f8251e4a1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -768,6 +768,7 @@ MulticopterPositionControl::run() // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller). publish_local_pos_sp(local_pos_sp); + _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller // Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint. _att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());