From d6b6a1d0b37c0c8085f5d9ca614b51588124e24c Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 5 Sep 2018 16:05:53 +0200 Subject: [PATCH] addressed comments from review Signed-off-by: Roman --- src/lib/FlightTasks/FlightTasks.hpp | 2 +- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 6 +++++- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 8 ++++++++ .../FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp | 6 ------ .../FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp | 7 ------- src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp | 2 +- .../tasks/ManualStabilized/FlightTaskManualStabilized.hpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 8 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index f0ce8974ee..da29a1be58 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -129,7 +129,7 @@ public: /** * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy. */ - void set_yaw_handler(WeatherVane *ext_yaw_handler) {_current_task.task->set_yaw_handler(ext_yaw_handler);} + void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);} private: diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 127586de0f..40c9b55dc8 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -193,7 +193,11 @@ bool FlightTaskAuto::_evaluateTriplets() } // set heading - if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) { + if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { + _yaw_setpoint = _yaw; + _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); + + } else if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) { _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; _yaw_setpoint = NAN; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index a7c1158d56..c50e63e044 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -78,6 +78,11 @@ public: bool activate() override; bool updateInitialize() 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;} + protected: void _setDefaultConstraints() override; float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */ @@ -122,6 +127,9 @@ 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 */ + bool _evaluateTriplets(); /**< Checks and sets triplets. */ bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */ bool _evaluateGlobalReference(); /**< Check is global reference is available. */ diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 922201fd90..630e858ac6 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -62,12 +62,6 @@ bool FlightTaskAutoMapper::update() _reset(); } - // check if an external yaw handler is active and if yes, let it compute the yaw setpoints - if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { - _yaw_setpoint = _yaw; - _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); - } - // The only time a thrust set-point is sent out is during // idle. Hence, reset thrust set-point to NAN in case the // vehicle exits idle. diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 6413370e52..efff0f6db0 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -50,11 +50,6 @@ public: bool activate() override; bool update() override; - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void set_yaw_handler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} - protected: float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ @@ -78,8 +73,6 @@ protected: void updateParams() override; /**< See ModuleParam class */ private: - WeatherVane *_ext_yaw_handler = - nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ void _reset(); /**< Resets member variables to current vehicle state */ WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index c8500602a0..4cb11b2baa 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -145,7 +145,7 @@ public: * 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 set_yaw_handler(WeatherVane *ext_yaw_handler) {}; + virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}; protected: diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp index 7372f70c62..b846a1ba42 100644 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp +++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp @@ -55,7 +55,7 @@ public: /** * Sets an external yaw handler which can be used to implement a different yaw control strategy. */ - void set_yaw_handler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} + void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} protected: virtual void _updateSetpoints(); /**< updates all setpoints*/ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d6ceb28feb..17a33d66ea 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -625,7 +625,7 @@ MulticopterPositionControl::task_main() // setpoints from flighttask vehicle_local_position_setpoint_s setpoint; - _flight_tasks.set_yaw_handler(&_wv_controller); + _flight_tasks.setYawHandler(&_wv_controller); // update task if (!_flight_tasks.update()) {