From de59d6c7881b22bd0bee943c47b5aac33da7b8f9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Aug 2020 12:42:36 -0400 Subject: [PATCH] flight_tasks: pass previous setpoint as const reference - perform setpoint checks in place to simplify and avoid copy --- .../tasks/Auto/FlightTaskAuto.cpp | 2 +- .../tasks/Auto/FlightTaskAuto.hpp | 2 +- .../FlightTaskAutoLineSmoothVel.cpp | 49 +++++++------------ .../FlightTaskAutoLineSmoothVel.hpp | 4 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 2 +- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 2 +- .../tasks/Descend/FlightTaskDescend.cpp | 2 +- .../tasks/Descend/FlightTaskDescend.hpp | 2 +- .../tasks/Failsafe/FlightTaskFailsafe.cpp | 2 +- .../tasks/Failsafe/FlightTaskFailsafe.hpp | 2 +- .../tasks/FlightTask/FlightTask.cpp | 2 +- .../tasks/FlightTask/FlightTask.hpp | 2 +- .../FlightTaskManualAltitude.cpp | 2 +- .../FlightTaskManualAltitude.hpp | 2 +- .../FlightTaskManualAltitudeSmoothVel.cpp | 26 +++++----- .../FlightTaskManualAltitudeSmoothVel.hpp | 4 +- .../FlightTaskManualPosition.cpp | 2 +- .../FlightTaskManualPosition.hpp | 2 +- .../FlightTaskManualPositionSmoothVel.cpp | 46 +++++++---------- .../FlightTaskManualPositionSmoothVel.hpp | 4 +- .../tasks/Offboard/FlightTaskOffboard.cpp | 2 +- .../tasks/Offboard/FlightTaskOffboard.hpp | 2 +- .../tasks/Orbit/FlightTaskOrbit.cpp | 2 +- .../tasks/Orbit/FlightTaskOrbit.hpp | 2 +- .../tasks/Transition/FlightTaskTransition.cpp | 15 ++---- .../tasks/Transition/FlightTaskTransition.hpp | 4 +- 26 files changed, 71 insertions(+), 117 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index 2c5855533b..b711261347 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -48,7 +48,7 @@ FlightTaskAuto::FlightTaskAuto() : } -bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); _position_setpoint = _position; diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp index 16c88fbaa0..ec31f61d06 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp @@ -83,7 +83,7 @@ public: FlightTaskAuto(); virtual ~FlightTaskAuto() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool updateFinalize() override; diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 4343f25054..f411b40f68 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -41,19 +41,30 @@ using namespace matrix; -bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTaskAutoMapper::activate(last_setpoint); - checkSetpoints(last_setpoint); - 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); + Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; + Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; + Vector3f accel_prev{last_setpoint.acceleration}; - for (int i = 0; i < 3; ++i) { - _trajectory[i].reset(last_setpoint.acceleration[i], vel_prev(i), pos_prev(i)); + for (int i = 0; i < 3; i++) { + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } + + // No acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } } - _yaw_sp_prev = last_setpoint.yaw; + for (int i = 0; i < 3; ++i) { + _trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); + } + + _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; _updateTrajConstraints(); return ret; @@ -69,30 +80,6 @@ void FlightTaskAutoLineSmoothVel::reActivate() _trajectory[2].reset(0.f, 0.7f, _position(2)); } -void FlightTaskAutoLineSmoothVel::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 - for (int i = 0; i < 3; i++) { - if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; } - } - - if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } -} - /** * EKF reset handling functions * Those functions are called by the base FlightTask in diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 34d70c509d..1a4457ab60 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -49,13 +49,11 @@ public: FlightTaskAutoLineSmoothVel() = default; virtual ~FlightTaskAutoLineSmoothVel() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; void reActivate() override; protected: - void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - /** Reset position or velocity setpoints in case of EKF reset event */ void _ekfResetHandlerPositionXY() override; void _ekfResetHandlerVelocityXY() override; diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 3a7ad5d3fe..ae74752f4d 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -44,7 +44,7 @@ FlightTaskAutoMapper::FlightTaskAutoMapper() : _sticks(this) {}; -bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTaskAuto::activate(last_setpoint); _reset(); diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 457419708e..50f34db88a 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -48,7 +48,7 @@ class FlightTaskAutoMapper : public FlightTaskAuto public: FlightTaskAutoMapper(); virtual ~FlightTaskAutoMapper() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool update() override; protected: diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp index 735e4d2318..106ce9d08c 100644 --- a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp +++ b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp @@ -36,7 +36,7 @@ #include "FlightTaskDescend.hpp" -bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); // stay level to minimize horizontal drift diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp index 7e6035d9ab..5a6edae13c 100644 --- a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp +++ b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp @@ -46,7 +46,7 @@ public: virtual ~FlightTaskDescend() = default; bool update() override; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; private: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp index 2911f9e5f1..c3ec934911 100644 --- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp +++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -36,7 +36,7 @@ #include "FlightTaskFailsafe.hpp" -bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); _position_setpoint = _position; diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp index c8a9eadcfa..9c869525d9 100644 --- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp +++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp @@ -47,7 +47,7 @@ public: virtual ~FlightTaskFailsafe() = default; bool update() override; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; private: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index 5fcac4b003..025bd54970 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -10,7 +10,7 @@ const ekf_reset_counters_s FlightTask::zero_reset_counters = {}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; -bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint) { _resetSetpoints(); _setDefaultConstraints(); diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp index d6b2b54c17..df91fde401 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp @@ -80,7 +80,7 @@ public: * @param last_setpoint last output of the previous task * @return true on success, false on error */ - virtual bool activate(vehicle_local_position_setpoint_s last_setpoint); + virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint); /** * Call this to reset an active Flight Task diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index ed56725238..7d784b8f72 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -61,7 +61,7 @@ bool FlightTaskManualAltitude::updateInitialize() return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); } -bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); _yaw_setpoint = NAN; diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 13d5bc4311..60378b2a2a 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -48,7 +48,7 @@ class FlightTaskManualAltitude : public FlightTask public: FlightTaskManualAltitude(); virtual ~FlightTaskManualAltitude() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 7bc4c90a92..a1d3cef450 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -41,14 +41,22 @@ using namespace matrix; -bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTaskManualAltitude::activate(last_setpoint); // Check if the previous FlightTask provided setpoints - checkSetpoints(last_setpoint); - _smoothing.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z); + // If the position setpoint is unknown, set to the current postion + float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2); + + // If the velocity setpoint is unknown, set to the current velocity + float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2); + + // No acceleration estimate available, set to zero if the setpoint is NAN + float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f; + + _smoothing.reset(az_sp_last, vz_sp_last, z_sp_last); return ret; } @@ -60,18 +68,6 @@ void FlightTaskManualAltitudeSmoothVel::reActivate() _smoothing.reset(0.f, 0.f, _position(2)); } -void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) -{ - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } - - // If the velocity setpoint is unknown, set to the current velocity - 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.acceleration[2])) { setpoints.acceleration[2] = 0.f; } -} - void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ() { _smoothing.setCurrentPosition(_position(2)); diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 2818ef37af..fee42dc341 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -48,7 +48,7 @@ public: FlightTaskManualAltitudeSmoothVel() = default; virtual ~FlightTaskManualAltitudeSmoothVel() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; void reActivate() override; protected: @@ -65,8 +65,6 @@ protected: ) private: - void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - void _updateTrajConstraints(); void _setOutputState(); diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index b447ed9c1e..3944cc126b 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -56,7 +56,7 @@ bool FlightTaskManualPosition::updateInitialize() && PX4_ISFINITE(_velocity(1)); } -bool FlightTaskManualPosition::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint) { // all requirements from altitude-mode still have to hold bool ret = FlightTaskManualAltitude::activate(last_setpoint); diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index ab01968057..0d0594aed4 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -49,7 +49,7 @@ public: FlightTaskManualPosition(); virtual ~FlightTaskManualPosition() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool updateInitialize() override; /** diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index b526a27694..183d6203bb 100644 --- a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -38,18 +38,28 @@ using namespace matrix; -bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTaskManualPosition::activate(last_setpoint); // Check if the previous FlightTask provided setpoints - checkSetpoints(last_setpoint); - const Vector2f accel_prev(last_setpoint.acceleration[0], last_setpoint.acceleration[1]); - const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy); - const Vector2f pos_prev(last_setpoint.x, last_setpoint.y); + Vector3f accel_prev{last_setpoint.acceleration}; + Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; + Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; - _smoothing_xy.reset(accel_prev, vel_prev, pos_prev); - _smoothing_z.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z); + for (int i = 0; i < 3; i++) { + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } + + // No acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + } + + _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); + _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); return ret; } @@ -62,28 +72,6 @@ void FlightTaskManualPositionSmoothVel::reActivate() _smoothing_z.reset(0.f, 0.f, _position(2)); } -void FlightTaskManualPositionSmoothVel::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 - for (int i = 0; i < 3; i++) { - if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; } - } -} - void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY() { _smoothing_xy.setCurrentPosition(_position.xy()); diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index 5427563ec5..2338a41ee7 100644 --- a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -53,7 +53,7 @@ public: virtual ~FlightTaskManualPositionSmoothVel() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; void reActivate() override; protected: @@ -73,8 +73,6 @@ protected: ) private: - void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - void _updateTrajConstraints(); void _updateTrajConstraintsXY(); void _updateTrajConstraintsZ(); diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp index 8b6cd46a84..c7e0a2cb61 100644 --- a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -57,7 +57,7 @@ bool FlightTaskOffboard::updateInitialize() && PX4_ISFINITE(_velocity(1)); } -bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskOffboard::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); _position_setpoint = _position; diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp index 605be2a313..18a0e4901b 100644 --- a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp +++ b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp @@ -48,7 +48,7 @@ public: virtual ~FlightTaskOffboard() = default; bool update() override; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool updateInitialize() override; protected: diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index b9c3a099fc..3c4c48096f 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -154,7 +154,7 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a) return v * v < a * r; } -bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTaskManualAltitudeSmooth::activate(last_setpoint); _r = _radius_min; diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp index 32cad9b7a6..5611f39ece 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -53,7 +53,7 @@ public: virtual ~FlightTaskOrbit() = default; bool applyCommandParameters(const vehicle_command_s &command) override; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool update() override; /** diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp index 0288e79558..e595e310ec 100644 --- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp @@ -42,22 +42,13 @@ bool FlightTaskTransition::updateInitialize() return FlightTask::updateInitialize(); } -bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s last_setpoint) +bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint) { - checkSetpoints(last_setpoint); - _transition_altitude = last_setpoint.z; - _transition_yaw = last_setpoint.yaw; + _transition_altitude = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2); + _transition_yaw = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; return FlightTask::activate(last_setpoint); } -void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &setpoints) -{ - // If the setpoint is unknown, set to the current estimate - if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); } - - if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; } -} - bool FlightTaskTransition::update() { bool ret = FlightTask::update(); diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp index cd2e567756..255ee47f82 100644 --- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp +++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp @@ -47,13 +47,11 @@ public: FlightTaskTransition() = default; virtual ~FlightTaskTransition() = default; - bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; private: - void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - float _transition_altitude = 0.0f; float _transition_yaw = 0.0f; };