From 37e0f91c397d9aa41d9bb0dc1cea600630f38485 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 18 Dec 2017 06:40:50 +0100 Subject: [PATCH] FlightTaskSport: scale maximal stick input to maximal allowed velocity instead of being hardcoded for testing --- src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 4 ++-- src/lib/FlightTasks/tasks/FlightTaskManual.hpp | 5 +++-- src/lib/FlightTasks/tasks/FlightTaskSport.hpp | 10 ++++++++-- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index f4f718b361..3ef634a54f 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -48,12 +48,12 @@ using namespace matrix; FlightTaskManual::FlightTaskManual(control::SuperBlock *parent, const char *name) : FlightTask(parent, name), + _z_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false), + _z_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false), _xy_vel_man_expo(parent, "MPC_XY_MAN_EXPO", false), _z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false), _hold_dz(parent, "MPC_HOLD_DZ", false), _velocity_hor_manual(parent, "MPC_VEL_MANUAL", false), - _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), _man_yaw_max(parent, "MPC_MAN_Y_MAX", false) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 592506fb10..4e09faba02 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -70,6 +70,9 @@ protected: float _get_input_frame_yaw() { return _yaw; } virtual void _scaleVelocity(matrix::Vector3f &velocity); + control::BlockParamFloat _z_vel_max_up; /**< maximal vertical velocity when flying upwards with the stick */ + control::BlockParamFloat _z_vel_max_down; /**< maximal vertical velocity when flying downwards with the stick */ + private: uORB::Subscription *_sub_manual_control_setpoint{nullptr}; @@ -77,8 +80,6 @@ private: control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */ control::BlockParamFloat _velocity_hor_manual; /**< target velocity in manual controlled mode at full speed */ - control::BlockParamFloat _z_vel_max_up; /**< maximal vertical velocity when flying upwards with the stick */ - 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 */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp index d0e6143f29..daace5ea1e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp @@ -47,7 +47,8 @@ class FlightTaskSport : public FlightTaskManual { public: FlightTaskSport(control::SuperBlock *parent, const char *name) : - FlightTaskManual(parent, name) + FlightTaskManual(parent, name), + _velocity_hor_max(parent, "MPC_XY_VEL_MAX", false) { } virtual ~FlightTaskSport() = default; @@ -55,8 +56,13 @@ public: protected: void _scaleVelocity(matrix::Vector3f &velocity) override { - const matrix::Vector3f velocity_scale(20.f, 20.f, 5.f); + const matrix::Vector3f velocity_scale(_velocity_hor_max.get(), + _velocity_hor_max.get(), + (velocity(2) > 0.0f) ? _z_vel_max_down.get() : _z_vel_max_up.get()); velocity = velocity.emult(velocity_scale); } +private: + control::BlockParamFloat _velocity_hor_max; /**< maximal allowed horizontal speed, in sport mode full stick input*/ + };