From de872ca04723666b23f677be1a14c03b60230537 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 15 Dec 2017 08:06:14 +0100 Subject: [PATCH] FlightTaskManual: remove the smoothing from the task to add it later in a child class also separated the velocity scaling to enable sport mode --- .../FlightTasks/tasks/FlightTaskManual.cpp | 327 +----------------- .../FlightTasks/tasks/FlightTaskManual.hpp | 45 +-- 2 files changed, 15 insertions(+), 357 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 55b67e486f..e63580740c 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -55,23 +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), - _jerk_hor_max(parent, "MPC_JERK_MAX", false), - _jerk_hor_min(parent, "MPC_JERK_MIN", false), - _deceleration_hor_slow(parent, "MPC_DEC_HOR_SLOW", false), - _acceleration_hor_max(this, "MPC_ACC_HOR_MAX", false), - _acceleration_hor_manual(this, "MPC_ACC_HOR", false), - _acceleration_z_max_up(this, "MPC_ACC_UP_MAX", false), - _acceleration_z_max_down(this, "MPC_ACC_DOWN_MAX", false), - _rc_flt_smp_rate(parent, "RC_FLT_SMP_RATE", false), - _rc_flt_cutoff(parent, "RC_FLT_CUTOFF", false), - _manual_direction_change_hysteresis(false), - _filter_roll_stick(50.0f, 10.0f), - _filter_pitch_stick(50.0f, 10.0f) + _hold_max_z(parent, "MPC_HOLD_MAX_Z", false) { - _manual_direction_change_hysteresis.set_hysteresis_time_from(false, DIRECTION_CHANGE_TIME_US); - _filter_roll_stick.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get()); - _filter_pitch_stick.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get()); _hold_position = Vector3f(NAN, NAN, NAN); } @@ -113,17 +98,13 @@ bool FlightTaskManual::update() } /* rotate stick input to produce velocity setpoint in NED frame */ - Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), _sticks(3)); - velocity_setpoint = Dcmf(Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * velocity_setpoint; + Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), _sticks(2)); + velocity_setpoint = Dcmf(Eulerf(0.0f, 0.0f, _get_input_frame_yaw())) * velocity_setpoint; /* scale [0,1] length velocity vector to maximal manual speed (in m/s) */ - Vector3f vel_scale(_velocity_hor_manual.get(), - _velocity_hor_manual.get(), - (velocity_setpoint(2) > 0.0f) ? _z_vel_max_down.get() : _z_vel_max_up.get()); - velocity_setpoint = velocity_setpoint.emult(vel_scale); + _scaleVelocity(velocity_setpoint); /* smooth out velocity setpoint by slewrate and return it */ - vel_sp_slewrate(stick_xy, _sticks(3), velocity_setpoint); _setVelocitySetpoint(velocity_setpoint); /* handle position and altitude hold */ @@ -154,6 +135,14 @@ bool FlightTaskManual::update() return true; } +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) { @@ -175,295 +164,3 @@ bool FlightTaskManual::_evaluateSticks() return false; } } - -void FlightTaskManual::reset_slewrate_xy() -{ - _vel_sp_prev(0) = _velocity(0); - _vel_sp_prev(1) = _velocity(1); -} - -void FlightTaskManual::vel_sp_slewrate(const Vector2f &stick_xy, const float &stick_z, Vector3f &vel_sp) -{ - Vector2f vel_sp_xy(vel_sp(0), vel_sp(1)); - const Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1)); - const Vector2f vel_xy(_velocity(0), _velocity(1)); - const Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / _deltatime; - - const float acc_xy_max = get_acceleration_xy(stick_xy); - - /* limit total horizontal acceleration */ - if (acc_xy.length() > acc_xy_max) { - vel_sp_xy = acc_xy_max * acc_xy.normalized() * _deltatime + vel_sp_prev_xy; - vel_sp(0) = vel_sp_xy(0); - vel_sp(1) = vel_sp_xy(1); - } - - /* limit vertical acceleration */ - const float acc_z = (vel_sp(2) - _vel_sp_prev(2)) / _deltatime; - const float acc_z_max = math::sign(acc_z) * get_acceleration_z(stick_z); - - if (fabsf(acc_z) > fabsf(acc_z_max)) { - vel_sp(2) = acc_z_max * _deltatime + _vel_sp_prev(2); - } - - _vel_sp_prev = vel_sp; -} - -float FlightTaskManual::get_acceleration_xy(const Vector2f &stick_xy) -{ - /* - * In manual mode we consider four states with different acceleration handling: - * 1. user wants to stop/brake (lets go of sticks) - * 2. user wants to quickly change direction (> 90 degree) - * 3. user wants to accelerate - * 4. user wants to decelerate - */ - float acceleration_state_dependent_xy = 0.f; - const Vector2f vel_xy(_velocity(0), _velocity(1)); - - /* check input stick for zero or direction */ - const float stick_xy_norm = stick_xy.norm(); - const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON; - Vector2f stick_xy_unit = stick_xy; - - if (!stick_xy_zero) { - stick_xy_unit.normalize(); - } - - const float stick_xy_prev_norm = _stick_input_xy_prev.norm(); - const bool stick_xy_prev_zero = stick_xy_prev_norm <= FLT_EPSILON; - Vector2f stick_xy_prev_unit = _stick_input_xy_prev; - - if (!stick_xy_prev_zero) { - stick_xy_prev_unit.normalize(); - } - - /* check if stick direction changed more than 60 degree angle cos(60) = 0.5 */ - const bool previous_stick_aligned = (stick_xy_unit * stick_xy_prev_unit) > 0.5f; - - /* check acceleration */ - const bool do_acceleration = stick_xy_prev_zero || (previous_stick_aligned && - ((stick_xy_norm > stick_xy_prev_norm) || (fabsf(stick_xy_norm - 1.0f) < FLT_EPSILON))); - - const bool do_deceleration = (previous_stick_aligned && (stick_xy_norm <= stick_xy_prev_norm)); - - const bool do_direction_change = !previous_stick_aligned; - - stick_user_intention intention_xy; - - if (stick_xy_zero) { - /* we want to stop */ - intention_xy = brake; - - } else if (do_acceleration) { - /* we do manual acceleration */ - intention_xy = acceleration; - - } else if (do_deceleration) { - /* we do manual deceleration */ - intention_xy = deceleration; - - } else if (do_direction_change) { - /* we have a direction change */ - intention_xy = direction_change; - - } else { - /* catchall: acceleration */ - intention_xy = acceleration; - } - - /* - * execute the user intention - */ - - /* pilot starts to have brake intention */ - if ((_intention_xy_prev != brake) && (intention_xy == brake)) { - - if (_jerk_hor_max.get() > _jerk_hor_min.get()) { - /* allow jerk proportional to velocity */ - const float jerk_range = _jerk_hor_max.get() - _jerk_hor_min.get(); - const float velocity_ratio = vel_xy.norm() / _velocity_hor_manual.get(); - _manual_jerk_limit_xy = _jerk_hor_min.get() + jerk_range * velocity_ratio; - - /* start braking with lowest deceleration */ - acceleration_state_dependent_xy = _deceleration_hor_slow.get(); - - } else { - /* set the jerk limit large because it's disabled*/ - _manual_jerk_limit_xy = 1e6f; - /* to brake we use max acceleration */ - acceleration_state_dependent_xy = _acceleration_hor_max.get(); - } - - reset_slewrate_xy(); - } - - /* allowed state transitions */ - switch (_intention_xy_prev) { - case brake: { - - /* pilot does not want to brake anymore, initialize with lowest acceleration */ - if (intention_xy != brake) { - _intention_xy_prev = acceleration; - acceleration_state_dependent_xy = _deceleration_hor_slow.get(); - } - - break; - } - - case direction_change: { - /* only exit direction change if brake or desired heading aligned */ - const bool stick_vel_aligned = (vel_xy * stick_xy_unit > 0.0f); - _manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned); - - if (intention_xy == brake) { - _intention_xy_prev = intention_xy; - - } else if (stick_vel_aligned) { - _intention_xy_prev = acceleration; - - } else if (_manual_direction_change_hysteresis.get_state()) { - - /* TODO: find conditions which are always continuous - * only if stick input is large*/ - if (stick_xy_norm > 0.6f) { - acceleration_state_dependent_xy = _acceleration_hor_max.get(); - } - } - - break; - } - - case acceleration: { - _intention_xy_prev = intention_xy; - - if (_intention_xy_prev == direction_change) { - reset_slewrate_xy(); - } - - break; - } - - case deceleration: { - _intention_xy_prev = intention_xy; - - if (_intention_xy_prev == direction_change) { - reset_slewrate_xy(); - } - - break; - } - } - - /* - * calculate dynamic acceleration based on state - */ - - switch (_intention_xy_prev) { - case brake: { - /* limit jerk when braking to zero */ - float jerk = (_acceleration_hor_max.get() - acceleration_state_dependent_xy) / _deltatime; - - if (jerk > _manual_jerk_limit_xy) { - acceleration_state_dependent_xy += _manual_jerk_limit_xy * _deltatime; - - } else { - acceleration_state_dependent_xy = _acceleration_hor_max.get(); - } - - break; - } - - case direction_change: { - /* limit acceleration linearly on stick input*/ - const float acceleration_range = _acceleration_hor_manual.get() - _deceleration_hor_slow.get(); - acceleration_state_dependent_xy = _deceleration_hor_slow.get() + acceleration_range * stick_xy_norm; - break; - } - - case acceleration: { - /* limit acceleration linearly on stick input*/ - float acc_limit = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy_norm - + _deceleration_hor_slow.get(); - - if (acceleration_state_dependent_xy > acc_limit) { - acc_limit = acceleration_state_dependent_xy; - } - - acceleration_state_dependent_xy = acc_limit; - break; - } - - case deceleration: { - acceleration_state_dependent_xy = _deceleration_hor_slow.get(); - break; - } - - } - - /* update previous stick input */ - _stick_input_xy_prev = Vector2f(_filter_pitch_stick.apply(stick_xy(0)), _filter_roll_stick.apply(stick_xy(1))); - - if (_stick_input_xy_prev.length() > 1.0f) { - _stick_input_xy_prev = _stick_input_xy_prev.normalized(); - } - - return acceleration_state_dependent_xy; -} - -float FlightTaskManual::get_acceleration_z(const float &stick_z) -{ - /* in manual altitude control apply acceleration limit based on stick input - * we consider two states - * 1.) brake - * 2.) accelerate */ - float acceleration_state_dependent_z = 0.f; - - /* default is acceleration */ - stick_user_intention intention = acceleration; - - if (fabsf(stick_z) <= FLT_EPSILON) { - intention = brake; - } - - /* set maximum acceleration depending on upwards or downwards flight */ - float max_acceleration = (stick_z <= 0.0f) ? _acceleration_z_max_up.get() : _acceleration_z_max_down.get(); - - /* - * update user input - */ - if ((_user_intention_z != brake) && (intention == brake)) { - - /* we start with lowest acceleration */ - acceleration_state_dependent_z = _acceleration_z_max_down.get(); - - /* reset slew rate */ - _vel_sp_prev(2) = _velocity(2); - _user_intention_z = brake; - } - - _user_intention_z = intention; - - /* - * apply acceleration depending on state - */ - if (_user_intention_z == brake) { - - /* limit jerk when braking to zero */ - float jerk = (_acceleration_z_max_up.get() - acceleration_state_dependent_z) / _deltatime; - - if (jerk > _manual_jerk_limit_z) { - acceleration_state_dependent_z += _manual_jerk_limit_z * _deltatime; - - } else { - acceleration_state_dependent_z = _acceleration_z_max_up.get(); - } - } - - if (_user_intention_z == acceleration) { - const float acceleration_range = max_acceleration - _acceleration_z_max_down.get(); - acceleration_state_dependent_z = _acceleration_z_max_down.get() + acceleration_range * fabsf(stick_z); - } - - return acceleration_state_dependent_z; -} diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 0f47613837..ae7cf05c0a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -34,7 +34,7 @@ /** * @file FlightTaskManual.hpp * - * Flight task for the normal, legacy, manual position controlled flight + * Flight task for the normal, manual position controlled flight * where stick inputs map basically to the velocity setpoint * * @author Matthias Grob @@ -43,8 +43,6 @@ #pragma once #include "FlightTask.hpp" -#include -#include #include class FlightTaskManual : public FlightTask @@ -67,7 +65,8 @@ protected: bool _evaluateSticks(); bool _sticks_data_required = true; /**< let sibling task define if it depends on stick data */ - float get_input_frame_yaw() { return _yaw; } + float _get_input_frame_yaw() { return _yaw; } + virtual void _scaleVelocity(matrix::Vector3f &velocity); private: uORB::Subscription *_sub_manual_control_setpoint{nullptr}; @@ -85,42 +84,4 @@ private: bool _evaluate_sticks(); - - /* --- Acceleration Smoothing --- */ - static constexpr uint64_t DIRECTION_CHANGE_TIME_US = 1e5; /**< Time in us to switch into direction change state */ - - control::BlockParamFloat _jerk_hor_max; /**< maximum jerk only applied when braking to zero */ - control::BlockParamFloat _jerk_hor_min; /**< minimum jerk only applied when braking to zero */ - control::BlockParamFloat _deceleration_hor_slow; /**< slow velocity setpoint slewrate for manual deceleration*/ - control::BlockParamFloat _acceleration_hor_max; /**< maximum velocity setpoint slewrate for auto & fast manual brake */ - control::BlockParamFloat _acceleration_hor_manual; /**< maximum velocity setpoint slewrate for manual acceleration */ - control::BlockParamFloat _acceleration_z_max_up; /**< max acceleration up */ - control::BlockParamFloat _acceleration_z_max_down; /**< max acceleration down */ - control::BlockParamFloat _rc_flt_smp_rate; /**< sample rate for stick lowpass filter */ - control::BlockParamFloat _rc_flt_cutoff; /**< cutoff frequency for stick lowpass filter */ - - matrix::Vector2f _stick_input_xy_prev; - matrix::Vector3f _vel_sp_prev; /**< velocity setpoint of last loop to calculate setpoint slewrate - acceleration */ - enum stick_user_intention { - brake, - direction_change, - acceleration, - deceleration - }; - stick_user_intention _intention_xy_prev = brake; /**< user intension derived from the xy stick input */ - stick_user_intention _user_intention_z = brake; /**< user intension derived from the z stick input */ - float _manual_jerk_limit_xy = 1.f; /**< jerk limit in manual mode dependent on stick input */ - float _manual_jerk_limit_z = 1.f; /**< jerk limit in manual mode in z */ - systemlib::Hysteresis _manual_direction_change_hysteresis; - math::LowPassFilter2p _filter_roll_stick; - math::LowPassFilter2p _filter_pitch_stick; - - void vel_sp_slewrate(const matrix::Vector2f &stick_xy, const float &stick_z, matrix::Vector3f &vel_sp); - - void reset_slewrate_xy(); - - float get_acceleration_xy(const matrix::Vector2f &stick_xy); - - float get_acceleration_z(const float &stick_z); - };