diff --git a/src/lib/weather_vane/WeatherVane.cpp b/src/lib/weather_vane/WeatherVane.cpp index b96fbc20c3..162ff8a78c 100644 --- a/src/lib/weather_vane/WeatherVane.cpp +++ b/src/lib/weather_vane/WeatherVane.cpp @@ -41,24 +41,45 @@ #include -WeatherVane::WeatherVane() : - ModuleParams(nullptr) +WeatherVane::WeatherVane(ModuleParams *parent) : + ModuleParams(parent) { } -void WeatherVane::update(const matrix::Vector3f &dcm_z_sp_prev, float yaw) +void WeatherVane::update() { - _dcm_z_sp_prev = dcm_z_sp_prev; - _yaw = yaw; + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _is_vtol = vehicle_status.is_vtol; + } + + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _flag_control_manual_enabled = vehicle_control_mode.flag_control_manual_enabled; + _flag_control_position_enabled = vehicle_control_mode.flag_control_position_enabled; + } + + // Weathervane is only enabled for VTOLs if it's enabled by parameter + // in manual we use weathervane just if position is controlled as well + // in mission we use weathervane except for when navigator disables it + _is_active = _is_vtol && _param_wv_en.get() + && ((_flag_control_manual_enabled && _flag_control_position_enabled) + || (!_flag_control_manual_enabled && !_navigator_force_disabled)); } -float WeatherVane::get_weathervane_yawrate() +float WeatherVane::getWeathervaneYawrate() { // direction of desired body z axis represented in earth frame - matrix::Vector3f body_z_sp(_dcm_z_sp_prev); + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + _vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint); + matrix::Vector3f body_z_sp(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z()); // attitude setpoint body z axis // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. - matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -_yaw); + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -vehicle_local_position.heading); body_z_sp = R_yaw * body_z_sp; body_z_sp.normalize(); diff --git a/src/lib/weather_vane/WeatherVane.hpp b/src/lib/weather_vane/WeatherVane.hpp index 465a7f3d3b..bf7917f3f0 100644 --- a/src/lib/weather_vane/WeatherVane.hpp +++ b/src/lib/weather_vane/WeatherVane.hpp @@ -44,33 +44,40 @@ #include #include +#include +#include +#include +#include +#include class WeatherVane : public ModuleParams { public: - WeatherVane(); + WeatherVane(ModuleParams *parent); ~WeatherVane() = default; - void activate() {_is_active = true;} + void setNavigatorForceDisabled(bool navigator_force_disabled) { _navigator_force_disabled = navigator_force_disabled; }; - void deactivate() {_is_active = false;} + bool isActive() {return _is_active;} - bool is_active() {return _is_active;} + void update(); - bool weathervane_enabled() { return _param_wv_en.get(); } - - void update(const matrix::Vector3f &dcm_z_sp_prev, float yaw); - - float get_weathervane_yawrate(); - - void update_parameters() { ModuleParams::updateParams(); } + float getWeathervaneYawrate(); private: - matrix::Vector3f _dcm_z_sp_prev; ///< previous attitude setpoint body z axis - float _yaw = 0.0f; ///< current yaw angle - - bool _is_active = true; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + bool _is_active{false}; + + // local copies of status such that we don't need to copy uORB messages all the time + bool _is_vtol{false}; + bool _flag_control_manual_enabled{false}; + bool _flag_control_position_enabled{false}; + bool _navigator_force_disabled{false}; DEFINE_PARAMETERS( (ParamBool) _param_wv_en, diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 10bfed9b00..72876087a3 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -60,7 +60,6 @@ FlightModeManager::~FlightModeManager() _current_task.task->~FlightTask(); } - delete _wv_controller; perf_free(_loop_perf); } @@ -107,33 +106,7 @@ void FlightModeManager::Run() _home_position_sub.update(); _vehicle_control_mode_sub.update(); _vehicle_land_detected_sub.update(); - - if (_vehicle_status_sub.update()) { - if (_vehicle_status_sub.get().is_vtol && (_wv_controller == nullptr)) { - // if vehicle is a VTOL we want to enable weathervane capabilities - _wv_controller = new WeatherVane(); - } - } - - // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy - // that turns the nose of the vehicle into the wind - if (_wv_controller != nullptr) { - - // in manual mode we just want to use weathervane if position is controlled as well - // in mission, enabling wv is done in flight task - if (_vehicle_control_mode_sub.get().flag_control_manual_enabled) { - if (_vehicle_control_mode_sub.get().flag_control_position_enabled && _wv_controller->weathervane_enabled()) { - _wv_controller->activate(); - - } else { - _wv_controller->deactivate(); - } - } - - vehicle_attitude_setpoint_s vehicle_attitude_setpoint; - _vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint); - _wv_controller->update(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z(), vehicle_local_position.heading); - } + _vehicle_status_sub.update(); start_flight_task(); @@ -157,10 +130,6 @@ void FlightModeManager::updateParams() if (isAnyTaskActive()) { _current_task.task->handleParameterUpdate(); } - - if (_wv_controller != nullptr) { - _wv_controller->update_parameters(); - } } void FlightModeManager::start_flight_task() @@ -461,8 +430,6 @@ void FlightModeManager::handleCommand() void FlightModeManager::generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position) { - _current_task.task->setYawHandler(_wv_controller); - // If the task fails sned out empty NAN setpoints and the controller will emergency failsafe trajectory_setpoint_s setpoint = FlightTask::empty_setpoint; vehicle_constraints_s constraints = FlightTask::empty_constraints; diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 9d8a6cb835..86dbc64913 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -131,7 +131,6 @@ private: FlightTaskIndex index{FlightTaskIndex::None}; } _current_task{}; - WeatherVane *_wv_controller{nullptr}; int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED}; int _task_failure_count{0}; diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 3d0bc84460..8d3ec4d7bc 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility) +target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 4952d61e14..ab43a28b42 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -40,14 +40,6 @@ using namespace matrix; -FlightTaskAuto::FlightTaskAuto() : - _obstacle_avoidance(this), - _sticks(this), - _stick_acceleration_xy(this) -{ - -} - bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); @@ -257,9 +249,7 @@ void FlightTaskAuto::_prepareLandSetpoints() vertical_speed *= (1 + _sticks.getPositionExpo()(2)); // Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone - const bool weather_vane_active = (_ext_yaw_handler != nullptr) && _ext_yaw_handler->is_active(); - - if (!weather_vane_active || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) { + if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) { _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); } @@ -455,11 +445,8 @@ bool FlightTaskAuto::_evaluateTriplets() _next_was_valid = _sub_triplet_setpoint.get().next.valid; } - if (_ext_yaw_handler != nullptr) { - // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) - (_param_wv_en.get() && !_sub_triplet_setpoint.get().current.disable_weather_vane) ? _ext_yaw_handler->activate() : - _ext_yaw_handler->deactivate(); - } + // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) + _weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane); // Calculate the current vehicle state and check if it has updated. State previous_state = _current_state; @@ -476,13 +463,15 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_next_wp, _sub_triplet_setpoint.get().next.yaw, _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN, - _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type); + _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); _obstacle_avoidance.checkAvoidanceProgress( _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); } // set heading - if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { + _weathervane.update(); + + if (_weathervane.isActive()) { _yaw_setpoint = NAN; // use the yawrate setpoint from WV only if not moving lateral (velocity setpoint below half of _param_mpc_xy_cruise) // otherwise, keep heading constant (as output from WV is not according to wind in this case) @@ -492,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets() _yawspeed_setpoint = 0.0f; } else { - _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); + _yawspeed_setpoint = _weathervane.getWeathervaneYawrate(); } diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index dd0ebeb59d..c650eb0e95 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include "Sticks.hpp" @@ -84,18 +85,13 @@ enum class State { class FlightTaskAuto : public FlightTask { public: - FlightTaskAuto(); - + FlightTaskAuto() = default; virtual ~FlightTaskAuto() = default; bool activate(const trajectory_setpoint_s &last_setpoint) override; void reActivate() override; bool updateInitialize() override; bool update() override; - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} void overrideCruiseSpeed(const float cruise_speed_m_s) override; protected: @@ -143,12 +139,12 @@ protected: AlphaFilter _yawspeed_filter; bool _yaw_sp_aligned{false}; - ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */ + ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */ PositionSmoothing _position_smoothing; Vector3f _unsmoothed_velocity_setpoint; - Sticks _sticks; - StickAccelerationXY _stick_acceleration_xy; + Sticks _sticks{this}; + StickAccelerationXY _stick_acceleration_xy{this}; StickYaw _stick_yaw; matrix::Vector3f _land_position; float _land_heading; @@ -164,7 +160,6 @@ protected: (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold - (ParamBool) _param_wv_en, // enable/disable weather vane (VTOL) (ParamFloat) _param_mpc_acc_hor, // acceleration in flight (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max, @@ -208,8 +203,7 @@ private: float _reference_altitude{NAN}; /**< Altitude relative to ground. */ hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */ - WeatherVane *_ext_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - + WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */ bool _evaluateTriplets(); /**< Checks and sets triplets. */ diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 9591b70261..3126c1ba64 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -55,7 +55,6 @@ #include #include #include -#include struct ekf_reset_counters_s { uint8_t xy; @@ -163,11 +162,6 @@ public: updateParams(); } - /** - * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy. - * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method. - */ - virtual void setYawHandler(WeatherVane *ext_yaw_handler) {} virtual void overrideCruiseSpeed(const float cruise_speed_m_s) {} void updateVelocityControllerFeedback(const matrix::Vector3f &vel_sp, diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt index 5f445d595b..2e78cf9ba9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt @@ -36,4 +36,4 @@ px4_add_library(FlightTaskManualAcceleration ) target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility) +target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility WeatherVane) diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index 6d8e11f878..ec4ed9e277 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -39,10 +39,6 @@ using namespace matrix; -FlightTaskManualAcceleration::FlightTaskManualAcceleration() : - _stick_acceleration_xy(this) -{}; - bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint); @@ -78,13 +74,15 @@ bool FlightTaskManualAcceleration::update() _constraints.want_takeoff = _checkTakeoff(); // check if an external yaw handler is active and if yes, let it update the yaw setpoints - if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) { + _weathervane.update(); + + if (_weathervane.isActive()) { _yaw_setpoint = NAN; // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { // vehicle is steady - _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + _yawspeed_setpoint += _weathervane.getWeathervaneYawrate(); } } diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 7a4193299a..694bcb682f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -43,26 +43,22 @@ #include "FlightTaskManualAltitudeSmoothVel.hpp" #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" +#include class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel { public: - FlightTaskManualAcceleration(); + FlightTaskManualAcceleration() = default; virtual ~FlightTaskManualAcceleration() = default; bool activate(const trajectory_setpoint_s &last_setpoint) override; bool update() override; - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } - private: void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - StickAccelerationXY _stick_acceleration_xy; + StickAccelerationXY _stick_acceleration_xy{this}; StickYaw _stick_yaw; - WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt index 6125d5462b..53db727e96 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_library(FlightTaskManualPosition target_link_libraries(FlightTaskManualPosition PRIVATE CollisionPrevention + WeatherVane PUBLIC FlightTaskManualAltitude ) diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 5aedd0903e..f7d9345cbf 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -41,11 +41,6 @@ using namespace matrix; -FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this) -{ - -} - bool FlightTaskManualPosition::updateInitialize() { bool ret = FlightTaskManualAltitude::updateInitialize(); @@ -141,14 +136,15 @@ void FlightTaskManualPosition::_updateSetpoints() _updateXYlock(); // check for position lock - // check if an external yaw handler is active and if yes, let it update the yaw setpoints - if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) { + _weathervane.update(); + + if (_weathervane.isActive()) { _yaw_setpoint = NAN; // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { // vehicle is steady - _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + _yawspeed_setpoint += _weathervane.getWeathervaneYawrate(); } } } diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index ca882b619c..b730fd5466 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -41,23 +41,17 @@ #pragma once #include +#include #include "FlightTaskManualAltitude.hpp" class FlightTaskManualPosition : public FlightTaskManualAltitude { public: - FlightTaskManualPosition(); - + FlightTaskManualPosition() = default; virtual ~FlightTaskManualPosition() = default; bool activate(const trajectory_setpoint_s &last_setpoint) override; bool updateInitialize() override; - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } - - protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -71,8 +65,6 @@ protected: private: uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ - WeatherVane *_weathervane_yaw_handler = - nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - - CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ + WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index c24a38f055..aa82261e9d 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -50,7 +50,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition { public: FlightTaskManualPositionSmoothVel() = default; - virtual ~FlightTaskManualPositionSmoothVel() = default; bool activate(const trajectory_setpoint_s &last_setpoint) override;