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{}; };