From 975ac11635b513afffce533dce0f3dc1fa682b11 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 16 Dec 2017 07:59:52 +0100 Subject: [PATCH] mc_pos_control: enable yawspeed execution for tasks --- src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 8 ++++++-- src/lib/FlightTasks/tasks/FlightTaskManual.hpp | 3 ++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index ab9dc15bed..89b1f1a404 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -55,7 +55,8 @@ FlightTaskManual::FlightTaskManual(control::SuperBlock *parent, const char *name _z_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false), _z_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false), _hold_max_xy(parent, "MPC_HOLD_MAX_XY", false), - _hold_max_z(parent, "MPC_HOLD_MAX_Z", false) + _hold_max_z(parent, "MPC_HOLD_MAX_Z", false), + _man_yaw_max(parent, "MPC_MAN_Y_MAX", false) { } bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array) @@ -146,7 +147,10 @@ bool FlightTaskManual::update() void FlightTaskManual::_updateYaw() { - _hold_yaw += _sticks(3) * _deltatime; + const float yaw_speed = _sticks(3) * math::radians(_man_yaw_max.get()); + _setYawspeedSetpoint(yaw_speed); + + _hold_yaw += yaw_speed * _deltatime; _setYawSetpoint(_hold_yaw); } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 06fcf95679..592506fb10 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -81,9 +81,10 @@ private: control::BlockParamFloat _z_vel_max_down; /**< maximal vertical velocity when flying downwards with the stick */ control::BlockParamFloat _hold_max_xy; /**< velocity threshold to switch into horizontal position hold */ control::BlockParamFloat _hold_max_z; /**< velocity threshold to switch into vertical position hold */ + control::BlockParamFloat _man_yaw_max; /**< maximal rotation speed around yaw axis with full stick input */ matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */ - float _hold_yaw; /**< absolute yaw which gets updated by the yawspeed input */ + float _hold_yaw = 0.f; /**< absolute yaw which gets updated by the yawspeed input */ void _updateYaw();