From 55b6f19367d7c0be28f4d80ce37c2f7b075cc701 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 22 Dec 2017 21:01:06 +0100 Subject: [PATCH] FlightTaskManual: remove stick_deadzone because base class already has it. Remove deadzone check for vel in xy/z because expo already contains deadzone --- src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 15 ++------------- src/lib/FlightTasks/tasks/FlightTaskManual.hpp | 2 +- .../tasks/FlightTaskManualAltitude.cpp | 8 +++----- .../tasks/FlightTaskManualAltitude.hpp | 1 - .../tasks/FlightTaskManualPosition.cpp | 2 +- 5 files changed, 7 insertions(+), 21 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 2d75f5f4e8..a626ea2b42 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -48,11 +48,9 @@ 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), + _hold_dz(parent, "MPC_HOLD_DZ", 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) + _z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false) { } @@ -126,15 +124,6 @@ void FlightTaskManual::_updateYaw() _setYawSetpoint(_hold_yaw); } -void FlightTaskManual::_scaleVelocity(Vector3f &velocity) -{ - const Vector3f velocity_scale(_velocity_hor_manual.get(), - _velocity_hor_manual.get(), - (velocity(2) > 0.0f) ? _z_vel_max_down.get() : _z_vel_max_up.get()); - - velocity = velocity.emult(velocity_scale); -} - bool FlightTaskManual::_evaluateSticks() { if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) { diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index c7d2fb8dcf..c002e44ee7 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -68,6 +68,7 @@ protected: bool _sticks_data_required = true; /**< let sibling task define if it depends on stick data */ matrix::Vector _sticks; /**< unmodified manual stick inputs */ matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/ + control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */ private: @@ -75,7 +76,6 @@ private: control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ 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 */ bool _evaluateSticks(); /**< checks and sets stick inputs */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 650a748965..26e3b34963 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -51,9 +51,7 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent, _vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false), _yaw_rate_scaling(parent, "MPC_MAN_Y_MAX", false), _acc_max_up(parent, "MPC_ACC_UP_MAX", false), - _acc_max_down(parent, "MPC_ACC_DOWN_MAX", false), - _stick_deadzone(parent, "MPC_HOLD_DZ", false) - + _acc_max_down(parent, "MPC_ACC_DOWN_MAX", false) {} bool FlightTaskManualAltitude::activate() @@ -79,7 +77,7 @@ void FlightTaskManualAltitude::scaleSticks() void FlightTaskManualAltitude::updateHeadingSetpoints() { - if (fabsf(_sticks(3)) < _stick_deadzone.get()) { + if (fabsf(_sticks(3)) < _hold_dz.get()) { /* Want to hold yaw */ _yaw_rate_sp = NAN; _yaw_sp = _yaw_sp_predicted; @@ -93,7 +91,7 @@ void FlightTaskManualAltitude::updateHeadingSetpoints() void FlightTaskManualAltitude::updateZsetpoints() { - if (fabsf(_sticks(2)) < _stick_deadzone.get()) { + if (fabsf(_sticks_expo(2)) < FLT_EPSILON) { /* Want to hold altitude */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index 298e69f3ff..a0a6a8ed7c 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -66,7 +66,6 @@ protected: control::BlockParamFloat _yaw_rate_scaling; /**< scaling factor from stick to yaw rate */ control::BlockParamFloat _acc_max_up; /**< maximum acceleration upward */ control::BlockParamFloat _acc_max_down; /**< maximum acceleration downward */ - control::BlockParamFloat _stick_deadzone; /**< stick is considered zero if below deadzone */ virtual void updateSetpoints(); /**< updates all setpoints */ virtual void scaleSticks(); /**< scales sticks to velocity */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index 458f55868f..dc88fed421 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -85,7 +85,7 @@ void FlightTaskManualPosition::updateXYsetpoints() { matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1)); - if (stick_xy.length() < _stick_deadzone.get()) { + if (stick_xy.length() < FLT_EPSILON) { /* Want to hold position */ if (_lock_time <= _lock_time_max) {