From 1414f50cea29ab77454d0bd5ad4dae3777ba573f Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 2 Jul 2019 16:11:44 +0200 Subject: [PATCH] FlightTask - When switching task, pass the last setpoints of the previous task to the new task This is done to allow proper initialization of the new FlightTask and give it a chance to continue the setpoints without discontinuity. The function checkSetpoints replaces the setpoints containing NANs with an estimate of the state. The estimate is usually the current estimate of the EKF or zero. The transition FlightTask also provides an estimate of the current acceleration to properly initialize the next FlightTask after back-transition. This avoid having to initialize the accelerations to zero knowing that the actual acceleration is usually far from zero. --- src/lib/FlightTasks/FlightTasks.cpp | 5 +- .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 4 +- .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 2 +- .../FlightTaskAutoLineSmoothVel.cpp | 11 ++-- .../FlightTaskAutoLineSmoothVel.hpp | 2 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 4 +- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 2 +- .../AutoMapper2/FlightTaskAutoMapper2.cpp | 4 +- .../AutoMapper2/FlightTaskAutoMapper2.hpp | 2 +- .../tasks/Failsafe/FlightTaskFailsafe.cpp | 4 +- .../tasks/Failsafe/FlightTaskFailsafe.hpp | 2 +- .../tasks/FlightTask/FlightTask.cpp | 45 +++++++++++++++- .../tasks/FlightTask/FlightTask.hpp | 5 +- .../FlightTaskManualAltitude.cpp | 4 +- .../FlightTaskManualAltitude.hpp | 2 +- .../FlightTaskManualAltitudeSmoothVel.cpp | 26 +++++----- .../FlightTaskManualAltitudeSmoothVel.hpp | 7 +-- .../FlightTaskManualPosition.cpp | 4 +- .../FlightTaskManualPosition.hpp | 2 +- .../FlightTaskManualPositionSmoothVel.cpp | 51 ++++++++----------- .../FlightTaskManualPositionSmoothVel.hpp | 10 +--- .../tasks/Offboard/FlightTaskOffboard.cpp | 4 +- .../tasks/Offboard/FlightTaskOffboard.hpp | 2 +- .../tasks/Orbit/FlightTaskOrbit.cpp | 4 +- .../tasks/Orbit/FlightTaskOrbit.hpp | 2 +- .../tasks/Sport/FlightTaskSport.hpp | 4 +- .../tasks/Transition/FlightTaskTransition.cpp | 31 ++++++++--- .../tasks/Transition/FlightTaskTransition.hpp | 5 +- 28 files changed, 151 insertions(+), 99 deletions(-) diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 1d792046ac..265b6c4bdf 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -64,6 +64,9 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) return 0; } + // Save current setpoints for the nex FlightTask + vehicle_local_position_setpoint_s current_state = getPositionSetpoint(); + if (_initTask(new_task_index)) { // invalid task return -1; @@ -85,7 +88,7 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) _subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions // activation failed - if (!_current_task.task->updateInitialize() || !_current_task.task->activate()) { + if (!_current_task.task->updateInitialize() || !_current_task.task->activate(current_state)) { _current_task.task->~FlightTask(); _current_task.task = nullptr; _current_task.index = FlightTaskIndex::None; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index fa50ce4ce3..617e4db85d 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -77,9 +77,9 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr return true; } -bool FlightTaskAuto::activate() +bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTask::activate(); + bool ret = FlightTask::activate(state_prev); _position_setpoint = _position; _velocity_setpoint = _velocity; _yaw_setpoint = _yaw_sp_prev = _yaw; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index fd36156bf4..f7e5fbeaa5 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -78,7 +78,7 @@ public: virtual ~FlightTaskAuto() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool updateInitialize() override; bool updateFinalize() override; diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index a4682c5d3a..e2bb34db49 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -41,12 +41,17 @@ using namespace matrix; -bool FlightTaskAutoLineSmoothVel::activate() +bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskAutoMapper2::activate(); + bool ret = FlightTaskAutoMapper2::activate(state_prev); + + checkSetpoints(state_prev); + const Vector3f accel_prev{state_prev.acc_x, state_prev.acc_y, state_prev.acc_z}; + const Vector3f vel_prev = Vector3f(state_prev.vx, state_prev.vy, state_prev.vz); + const Vector3f pos_prev = Vector3f(state_prev.x, state_prev.y, state_prev.z); for (int i = 0; i < 3; ++i) { - _trajectory[i].reset(0.f, _velocity(i), _position(i)); + _trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); } _yaw_sp_prev = _yaw; diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 17c4f5ad52..c4ddf26f80 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -49,7 +49,7 @@ public: FlightTaskAutoLineSmoothVel() = default; virtual ~FlightTaskAutoLineSmoothVel() = default; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; void reActivate() override; protected: diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 91feecace2..d0369754db 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -40,9 +40,9 @@ using namespace matrix; -bool FlightTaskAutoMapper::activate() +bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskAuto::activate(); + bool ret = FlightTaskAuto::activate(state_prev); _reset(); return ret; } diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 3811b3e7dc..e531594d28 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -47,7 +47,7 @@ class FlightTaskAutoMapper : public FlightTaskAuto public: FlightTaskAutoMapper() = default; virtual ~FlightTaskAutoMapper() = default; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool update() override; protected: diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index ab48ed5c06..b6fe78bc86 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -40,9 +40,9 @@ using namespace matrix; -bool FlightTaskAutoMapper2::activate() +bool FlightTaskAutoMapper2::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskAuto::activate(); + bool ret = FlightTaskAuto::activate(state_prev); _reset(); return ret; } diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp index 5b4e0603d9..6c32d4c72c 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp @@ -47,7 +47,7 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto public: FlightTaskAutoMapper2() = default; virtual ~FlightTaskAutoMapper2() = default; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool update() override; protected: diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp index 9a84ab2004..973d53242f 100644 --- a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp +++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -36,9 +36,9 @@ #include "FlightTaskFailsafe.hpp" -bool FlightTaskFailsafe::activate() +bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTask::activate(); + bool ret = FlightTask::activate(state_prev); _position_setpoint = _position; _velocity_setpoint.zero(); _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f); diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp index 4264b9b120..4ba47ffda8 100644 --- a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp +++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp @@ -47,7 +47,7 @@ public: virtual ~FlightTaskFailsafe() = default; bool update() override; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; private: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index 21155786f5..b8f269a99e 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -22,7 +22,7 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) return true; } -bool FlightTask::activate() +bool FlightTask::activate(vehicle_local_position_setpoint_s state_prev) { _resetSetpoints(); _setDefaultConstraints(); @@ -34,7 +34,7 @@ bool FlightTask::activate() void FlightTask::reActivate() { - activate(); + activate(getPositionSetpoint()); } bool FlightTask::updateInitialize() @@ -47,6 +47,47 @@ bool FlightTask::updateInitialize() return true; } +void FlightTask::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) +{ + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); } + + if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); } + + if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); } + + if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); } + + if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); } + + // No acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; } + + if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; } + + if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } + + // No jerk estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(setpoints.jerk_x)) { setpoints.jerk_x = 0.f; } + + if (!PX4_ISFINITE(setpoints.jerk_y)) { setpoints.jerk_y = 0.f; } + + if (!PX4_ISFINITE(setpoints.jerk_z)) { setpoints.jerk_z = 0.f; } + + if (!PX4_ISFINITE(setpoints.thrust[0])) { setpoints.thrust[0] = 0.f; } + + if (!PX4_ISFINITE(setpoints.thrust[1])) { setpoints.thrust[1] = 0.f; } + + if (!PX4_ISFINITE(setpoints.thrust[2])) { setpoints.thrust[2] = -0.5f; } + + if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } + + if (!PX4_ISFINITE(setpoints.yawspeed)) { setpoints.yawspeed = 0.f; } +} + const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() { /* fill position setpoint message */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index b6fd32ae0b..04f210ba71 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -76,9 +76,10 @@ public: /** * Call once on the event where you switch to the task + * @param state of the previous task * @return true on success, false on error */ - virtual bool activate(); + virtual bool activate(vehicle_local_position_setpoint_s state_prev); /** * Call this to reset an active Flight Task @@ -119,6 +120,8 @@ public: */ const vehicle_local_position_setpoint_s getPositionSetpoint(); + void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); + /** * Get vehicle constraints. * The constraints can vary with task. diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 433f0956e5..795e5635b8 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -61,9 +61,9 @@ bool FlightTaskManualAltitude::updateInitialize() return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); } -bool FlightTaskManualAltitude::activate() +bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskManual::activate(); + bool ret = FlightTaskManual::activate(state_prev); _yaw_setpoint = NAN; _yawspeed_setpoint = 0.0f; _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 13a808074d..4be2b6b231 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -48,7 +48,7 @@ public: FlightTaskManualAltitude() = default; virtual ~FlightTaskManualAltitude() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool updateInitialize() override; bool update() override; diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 17a9126813..df75642d99 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -41,11 +41,16 @@ using namespace matrix; -bool FlightTaskManualAltitudeSmoothVel::activate() +bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskManualAltitude::activate(); + bool ret = FlightTaskManualAltitude::activate(state_prev); - _reset(); + // Check if the previous FlightTask provided setpoints + checkSetpoints(state_prev); + + _smoothing.reset(state_prev.acc_z, state_prev.vz, state_prev.z); + + _resetPositionLock(); return ret; } @@ -54,20 +59,13 @@ void FlightTaskManualAltitudeSmoothVel::reActivate() { // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly // using the generated jerk, reset the z derivatives to zero - _reset(true); + _smoothing.reset(0.f, 0.f, _position(2)); + + _resetPositionLock(); } -void FlightTaskManualAltitudeSmoothVel::_reset(bool force_vz_zero) +void FlightTaskManualAltitudeSmoothVel::_resetPositionLock() { - // Set the z derivatives to zero - if (force_vz_zero) { - _smoothing.reset(0.f, 0.f, _position(2)); - - } else { - // TODO: get current accel - _smoothing.reset(0.f, _velocity(2), _position(2)); - } - // Always start unlocked _position_lock_z_active = false; _position_setpoint_z_locked = NAN; diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 9e8980b630..b6b1534d71 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -48,7 +48,7 @@ public: FlightTaskManualAltitudeSmoothVel() = default; virtual ~FlightTaskManualAltitudeSmoothVel() = default; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; void reActivate() override; protected: @@ -63,10 +63,7 @@ protected: private: - /** - * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states - */ - void _reset(bool force_vz_zero = false); + void _resetPositionLock(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ VelocitySmoothing _smoothing; ///< Smoothing in z direction diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 6b9c9c0d59..8f950e22ac 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -65,10 +65,10 @@ bool FlightTaskManualPosition::updateInitialize() && PX4_ISFINITE(_velocity(1)); } -bool FlightTaskManualPosition::activate() +bool FlightTaskManualPosition::activate(vehicle_local_position_setpoint_s state_prev) { // all requirements from altitude-mode still have to hold - bool ret = FlightTaskManualAltitude::activate(); + bool ret = FlightTaskManualAltitude::activate(state_prev); _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get()); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 2a5feefa4b..1d5d628830 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -50,7 +50,7 @@ public: virtual ~FlightTaskManualPosition() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool updateInitialize() override; /** diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 7fd9b10ce2..21271bac80 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -38,11 +38,21 @@ using namespace matrix; -bool FlightTaskManualPositionSmoothVel::activate() +bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskManualPosition::activate(); + bool ret = FlightTaskManualPosition::activate(state_prev); - reset(Axes::XYZ); + // Check if the previous FlightTask provided setpoints + checkSetpoints(last_setpoint); + const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z); + const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz); + const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z); + + for (int i = 0; i < 3; ++i) { + _smoothing[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); + } + + _resetPositionLock(); return ret; } @@ -51,37 +61,18 @@ void FlightTaskManualPositionSmoothVel::reActivate() { // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly // using the generated jerk, reset the z derivatives to zero - reset(Axes::XYZ, true); -} - -void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero) -{ - int count; - - switch (axes) { - case Axes::XY: - count = 2; - break; - - case Axes::XYZ: - count = 3; - break; - - default: - count = 0; - break; - } - - // TODO: get current accel - for (int i = 0; i < count; ++i) { + for (int i = 0; i < 2; ++i) { _smoothing[i].reset(0.f, _velocity(i), _position(i)); } - // Set the z derivatives to zero - if (force_z_zero) { - _smoothing[2].reset(0.f, 0.f, _position(2)); - } + _smoothing[2].reset(0.f, 0.f, _position(2)); + + _initEkfResetCounters(); + _resetPositionLock(); +} +void FlightTaskManualPositionSmoothVel::_resetPositionLock() +{ _position_lock_xy_active = false; _position_lock_z_active = false; _position_setpoint_xy_locked(0) = NAN; diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index d56564d7e3..465c7974fd 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -49,7 +49,7 @@ public: virtual ~FlightTaskManualPositionSmoothVel() = default; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; void reActivate() override; protected: @@ -62,13 +62,7 @@ protected: (ParamFloat) _param_mpc_acc_down_max ) private: - - enum class Axes {XY, XYZ}; - - /** - * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states - */ - void reset(Axes axes, bool force_z_zero = false); + void _resetPositionLock(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp index 8b05f17b47..c1502e52b2 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -64,9 +64,9 @@ bool FlightTaskOffboard::updateInitialize() && PX4_ISFINITE(_velocity(1)); } -bool FlightTaskOffboard::activate() +bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTask::activate(); + bool ret = FlightTask::activate(state_prev); _position_setpoint = _position; _velocity_setpoint.setZero(); _position_lock.setAll(NAN); diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp index fd42e6298f..969c0f4fea 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp @@ -49,7 +49,7 @@ public: virtual ~FlightTaskOffboard() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool update() override; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool updateInitialize() override; protected: diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index 428ab84b2f..8a91b5e9dd 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -147,9 +147,9 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a) return v * v < a * r; } -bool FlightTaskOrbit::activate() +bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s state_prev) { - bool ret = FlightTaskManualAltitudeSmooth::activate(); + bool ret = FlightTaskManualAltitudeSmooth::activate(state_prev); _r = _radius_min; _v = 1.f; _center = Vector2f(_position); diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp index f0e15d14c2..8e6b683877 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -51,7 +51,7 @@ public: virtual ~FlightTaskOrbit(); bool applyCommandParameters(const vehicle_command_s &command) override; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool update() override; /** diff --git a/src/lib/FlightTasks/tasks/Sport/FlightTaskSport.hpp b/src/lib/FlightTasks/tasks/Sport/FlightTaskSport.hpp index 98b978052b..9554ef1ab9 100644 --- a/src/lib/FlightTasks/tasks/Sport/FlightTaskSport.hpp +++ b/src/lib/FlightTasks/tasks/Sport/FlightTaskSport.hpp @@ -52,9 +52,9 @@ public: virtual ~FlightTaskSport() = default; - bool activate() override + bool activate(vehicle_local_position_setpoint_s state_prev) override { - bool ret = FlightTaskManualPosition::activate(); + bool ret = FlightTaskManualPosition::activate(state_prev); // default constraints already are the maximum allowed limits _setDefaultConstraints(); diff --git a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp index 6146e6c19c..6fa73f8d2c 100644 --- a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp @@ -42,14 +42,29 @@ bool FlightTaskTransition::updateInitialize() return FlightTask::updateInitialize(); } -bool FlightTaskTransition::activate() +bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s state_prev) { - // transition at the current altitude and current yaw at the time of activation - // it would be better to use the last setpoint from the previous running flighttask but that interface - // is not available - _transition_altitude = _position(2); - _transition_yaw = _yaw; - return FlightTask::activate(); + checkSetpoints(state_prev); + _transition_altitude = state_prev.z; + _transition_yaw = state_prev.yaw; + _acceleration_setpoint.setAll(0.f); + _velocity_prev = _velocity; + return FlightTask::activate(state_prev); +} + +void FlightTaskTransition::updateAccelerationEstimate() +{ + // Estimate the acceleration by filtering the raw derivative of the velocity estimate + // This is done to provide a good estimate of the current acceleration to the next flight task after back-transition + _acceleration_setpoint = 0.9f * _acceleration_setpoint + 0.1f * (_velocity - _velocity_prev) / _deltatime; + + if (!PX4_ISFINITE(_acceleration_setpoint(0)) || + !PX4_ISFINITE(_acceleration_setpoint(1)) || + !PX4_ISFINITE(_acceleration_setpoint(2))) { + _acceleration_setpoint.setAll(0.f); + } + + _velocity_prev = _velocity; } bool FlightTaskTransition::update() @@ -61,6 +76,8 @@ bool FlightTaskTransition::update() _velocity_setpoint *= NAN; _position_setpoint(2) = _transition_altitude; + updateAccelerationEstimate(); + _yaw_setpoint = _transition_yaw; return true; } diff --git a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp index c020b70a8d..2b6eb0950d 100644 --- a/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp +++ b/src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp @@ -47,11 +47,14 @@ public: FlightTaskTransition() = default; virtual ~FlightTaskTransition() = default; - bool activate() override; + bool activate(vehicle_local_position_setpoint_s state_prev) override; bool updateInitialize() override; bool update() override; private: + void updateAccelerationEstimate(); + float _transition_altitude = 0.0f; float _transition_yaw = 0.0f; + matrix::Vector3f _velocity_prev{}; };