From f405bf506bcf7169c34b51143e2fe009de4a3486 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 12 Sep 2019 15:00:46 +0200 Subject: [PATCH] AltitudeSmoothVel - Use ManualVelocitySmoothingZ class instead of re-implementing the logic --- .../FlightTaskManualAltitudeSmoothVel.cpp | 120 +++++------------- .../FlightTaskManualAltitudeSmoothVel.hpp | 11 +- 2 files changed, 40 insertions(+), 91 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index f63f53243d..9a1328d024 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -51,7 +51,6 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint _smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z); _initEkfResetCounters(); - _resetPositionLock(); return ret; } @@ -63,7 +62,6 @@ void FlightTaskManualAltitudeSmoothVel::reActivate() _smoothing.reset(0.f, 0.f, _position(2)); _initEkfResetCounters(); - _resetPositionLock(); } void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) @@ -78,19 +76,34 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } } -void FlightTaskManualAltitudeSmoothVel::_resetPositionLock() -{ - // Always start unlocked - _position_lock_z_active = false; - _position_setpoint_z_locked = NAN; -} - void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters() { _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; } +void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() +{ + // Reset trajectories if EKF did a reset + _checkEkfResetCounters(); + + // Set max accel/vel/jerk + // Has to be done before _updateTrajectories() + _updateTrajConstraints(); + + _smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2)); + _smoothing.setCurrentPositionEstimate(_position(2)); + + + // Get yaw setpoint, un-smoothed position setpoints + FlightTaskManualAltitude::_updateSetpoints(); + + _smoothing.update(_deltatime, _velocity_setpoint(2)); + + // Fill the jerk, acceleration, velocity and position setpoint vectors + _setOutputState(); +} + void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters() { if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { @@ -104,85 +117,22 @@ void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters() } } -void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() +void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints() { - float pos_sp_smooth; + _smoothing.setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing.updateTraj(_deltatime); + _smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get()); + _smoothing.setMaxVelUp(_constraints.speed_up); + _smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get()); + _smoothing.setMaxVelDown(_constraints.speed_down); +} + + +void FlightTaskManualAltitudeSmoothVel::_setOutputState() +{ _jerk_setpoint(2) = _smoothing.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); - _vel_sp_smooth = _smoothing.getCurrentVelocity(); - pos_sp_smooth = _smoothing.getCurrentPosition(); - - /* Get yaw setpont, un-smoothed position setpoints.*/ - FlightTaskManualAltitude::_updateSetpoints(); - - /* Update constraints */ - if (_velocity_setpoint(2) < 0.f) { // up - _smoothing.setMaxAccel(_param_mpc_acc_up_max.get()); - _smoothing.setMaxVel(_constraints.speed_up); - - } else { // down - _smoothing.setMaxAccel(_param_mpc_acc_down_max.get()); - _smoothing.setMaxVel(_constraints.speed_down); - } - - float jerk = _param_mpc_jerk_max.get(); - - _checkEkfResetCounters(); - - /* 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. - */ - - const float velocity_target_z = _velocity_setpoint(2); - - if (fabsf(velocity_target_z) > FLT_EPSILON) { - if (_position_lock_z_active) { - _smoothing.setCurrentVelocity(_velocity_setpoint_feedback( - 2)); // Start the trajectory at the current velocity setpoint - _position_setpoint_z_locked = NAN; - } - - _position_lock_z_active = false; - } - - _smoothing.setMaxJerk(jerk); - _smoothing.updateDurations(_velocity_setpoint(2)); - - if (!_position_lock_z_active) { - _smoothing.setCurrentPosition(_position(2)); - } - - _velocity_setpoint(2) = _vel_sp_smooth; // Feedforward - - if (fabsf(_vel_sp_smooth) < 0.1f && - fabsf(_acceleration_setpoint(2)) < .2f && - fabsf(velocity_target_z) <= FLT_EPSILON) { - _position_lock_z_active = true; - } - - // Set valid position setpoint while in position lock. - // When the position lock condition above is false, it does not - // mean that the unlock condition is true. This is why - // we are checking the lock flag here. - if (_position_lock_z_active) { - _position_setpoint_z_locked = pos_sp_smooth; - - // If the velocity setpoint is smaller than 1mm/s and that the acceleration is 0, force the setpoints - // to zero. This is required because the generated velocity is never exactly zero and if the drone hovers - // for a long period of time, thr drift of the position setpoint will be noticeable. - if (fabsf(_velocity_setpoint(2)) < 1e-3f && fabsf(_acceleration_setpoint(2)) < FLT_EPSILON) { - _velocity_setpoint(2) = 0.f; - _acceleration_setpoint(2) = 0.f; - _smoothing.setCurrentVelocity(0.f); - _smoothing.setCurrentAcceleration(0.f); - } - } - - _position_setpoint(2) = _position_setpoint_z_locked; + _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); + _position_setpoint(2) = _smoothing.getCurrentPosition(); } diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 608b286c7e..28bd8b37be 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -40,7 +40,7 @@ #pragma once #include "FlightTaskManualAltitude.hpp" -#include "VelocitySmoothing.hpp" +#include "ManualVelocitySmoothingZ.hpp" class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude { @@ -64,14 +64,13 @@ protected: private: void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - void _resetPositionLock(); + void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ + void _updateTrajConstraints(); + void _setOutputState(); - VelocitySmoothing _smoothing; ///< Smoothing in z direction - float _vel_sp_smooth; - bool _position_lock_z_active{false}; - float _position_setpoint_z_locked{NAN}; + ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction /* counters for estimator local position resets */ struct {