diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index f0e5221306..5b23a1a1d8 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -41,6 +41,8 @@ #pragma once +#include + class FlightTask : public control::SuperBlock { public: diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp new file mode 100644 index 0000000000..686e7dc5b0 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -0,0 +1,432 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightTaskManual.hpp + * + * Flight task for the normal, legacy, manual position controlled flight + * where stick inputs map basically to the velocity setpoint + * + * @author Matthias Grob + */ + +#include "FlightTaskManual.hpp" +#include + +using namespace matrix; + +int FlightTaskManual::activate() +{ + FlightTask::activate(); + _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); + return 0; +} + +int FlightTaskManual::disable() +{ + FlightTask::disable(); + return 0; +} + +int FlightTaskManual::update() +{ + int ret = FlightTask::update(); + ret += _evaluate_sticks(); + + /* prepare stick input */ + Vector2f stick_xy(_sticks.data()); /**< horizontal two dimensional stick input within a unit circle */ + + const float stick_xy_norm = stick_xy.norm(); + + /* saturate such that magnitude in xy is never larger than 1 */ + if (stick_xy_norm > 1.0f) { + stick_xy /= stick_xy_norm; + } + + /* 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; + + /* 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); + + /* smooth out velocity setpoint by slewrate and return it */ + vel_sp_slewrate(velocity_setpoint, stick_xy, _sticks(3)); + _set_velocity_setpoint(velocity_setpoint); + + /* handle position and altitude hold */ + const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON; + const bool stick_z_zero = fabsf(_sticks(3)) <= FLT_EPSILON; + + float velocity_xy_norm = Vector2f(_velocity.data()).norm(); + const bool stopped_xy = (_hold_max_xy.get() < FLT_EPSILON || velocity_xy_norm < _hold_max_xy.get()); + const bool stopped_z = (_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _hold_max_z.get()); + + if (stick_xy_zero && stopped_xy && !PX4_ISFINITE(_hold_position(0))) { + _hold_position(0) = _position(0); + _hold_position(1) = _position(1); + + } else if (!stick_xy_zero) { + _hold_position(0) = NAN; + _hold_position(1) = NAN; + } + + if (stick_z_zero && stopped_z && !PX4_ISFINITE(_hold_position(2))) { + _hold_position(2) = _position(2); + + } else if (!stick_z_zero) { + _hold_position(2) = NAN; + } + + _set_position_setpoint(_hold_position); + return ret; +} + +int FlightTaskManual::_evaluate_sticks() +{ + if (hrt_elapsed_time(&_sub_manual_control_setpoint.get().timestamp) < _timeout) { + /* get data and scale correctly */ + _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */ + _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */ + _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ + _sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */ + + /* apply expo and deadzone */ + _sticks(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); + _sticks(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); + _sticks(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); + + return 0; + + } else { + _sticks.zero(); /* default is all zero */ + return 1; + } +} + +void FlightTaskManual::reset_slewrate_xy() +{ + _vel_sp_prev(0) = _velocity(0); + _vel_sp_prev(1) = _velocity(1); +} + +void FlightTaskManual::vel_sp_slewrate(Vector3f &vel_sp, const Vector2f &stick_xy, const float &stick_z) +{ + 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 59884ea36e..9ec915fc43 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -43,6 +43,7 @@ #pragma once #include "FlightTask.hpp" +#include class FlightTaskManual : public FlightTask { @@ -75,97 +76,11 @@ public: }; virtual ~FlightTaskManual() {}; - /** - * Call once on the event where you switch to the task - * @return 0 on success, >0 on error otherwise - */ - virtual int activate() - { - FlightTask::activate(); - _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 = matrix::Vector3f(NAN, NAN, NAN); - return 0; - }; - - /** - * Call once on the event of switching away from the task - * @return 0 on success, >0 on error otherwise - */ - virtual int disable() - { - FlightTask::disable(); - return 0; - }; - - /** - * Call regularly in the control loop cycle to execute the task - * @return 0 on success, >0 on error otherwise - */ - virtual int update() - { - int ret = FlightTask::update(); - ret += _evaluate_sticks(); - - /* prepare stick input */ - matrix::Vector2f stick_xy(_sticks.data()); /**< horizontal two dimensional stick input within a unit circle */ - - const float stick_xy_norm = stick_xy.norm(); - - /* saturate such that magnitude in xy is never larger than 1 */ - if (stick_xy_norm > 1.0f) { - stick_xy /= stick_xy_norm; - } - - /* rotate stick input to produce velocity setpoint in NED frame */ - matrix::Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), _sticks(3)); - velocity_setpoint = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * velocity_setpoint; - - /* scale [0,1] length velocity vector to maximal manual speed (in m/s) */ - matrix::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); - - /* smooth out velocity setpoint by slewrate and return it */ - vel_sp_slewrate(velocity_setpoint, stick_xy, _sticks(3)); - _set_velocity_setpoint(velocity_setpoint); - - /* handle position and altitude hold */ - const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON; - const bool stick_z_zero = fabsf(_sticks(3)) <= FLT_EPSILON; - - float velocity_xy_norm = matrix::Vector2f(_velocity.data()).norm(); - const bool stopped_xy = (_hold_max_xy.get() < FLT_EPSILON || velocity_xy_norm < _hold_max_xy.get()); - const bool stopped_z = (_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _hold_max_z.get()); - - if (stick_xy_zero && stopped_xy && !PX4_ISFINITE(_hold_position(0))) { - _hold_position(0) = _position(0); - _hold_position(1) = _position(1); - - } else if (!stick_xy_zero) { - _hold_position(0) = NAN; - _hold_position(1) = NAN; - } - - if (stick_z_zero && stopped_z && !PX4_ISFINITE(_hold_position(2))) { - _hold_position(2) = _position(2); - - } else if (!stick_z_zero) { - _hold_position(2) = NAN; - } - - _set_position_setpoint(_hold_position); - return ret; - }; protected: matrix::Vector _sticks; - float get_input_frame_yaw() - { - return _yaw; - }; + float get_input_frame_yaw() { return _yaw; }; private: uORB::Subscription _sub_manual_control_setpoint; @@ -181,27 +96,7 @@ private: matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */ - int _evaluate_sticks() - { - if (hrt_elapsed_time(&_sub_manual_control_setpoint.get().timestamp) < _timeout) { - /* get data and scale correctly */ - _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */ - _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */ - _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ - _sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */ - - /* apply expo and deadzone */ - _sticks(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); - _sticks(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); - _sticks(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); - - return 0; - - } else { - _sticks.zero(); /* default is all zero */ - return 1; - } - } + int _evaluate_sticks(); /* --- Acceleration Smoothing --- */ @@ -233,296 +128,12 @@ private: math::LowPassFilter2p _filter_roll_stick; math::LowPassFilter2p _filter_pitch_stick; - void vel_sp_slewrate(matrix::Vector3f &vel_sp, const matrix::Vector2f &stick_xy, const float &stick_z) - { - matrix::Vector2f vel_sp_xy(vel_sp(0), vel_sp(1)); - const matrix::Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1)); - const matrix::Vector2f vel_xy(_velocity(0), _velocity(1)); - const matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / _deltatime; + void vel_sp_slewrate(matrix::Vector3f &vel_sp, const matrix::Vector2f &stick_xy, const float &stick_z); - const float acc_xy_max = get_acceleration_xy(stick_xy); + void reset_slewrate_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); - } + float get_acceleration_xy(const matrix::Vector2f &stick_xy); - /* 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; - } - - void reset_slewrate_xy() - { - _vel_sp_prev(0) = _velocity(0); - _vel_sp_prev(1) = _velocity(1); - } - - float get_acceleration_xy(const matrix::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 matrix::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; - matrix::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; - matrix::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 = matrix::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 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; - } + float get_acceleration_z(const float &stick_z); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp new file mode 100644 index 0000000000..33e6521ce0 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightTaskManual.hpp + * + * Flight task for the normal, legacy, manual position controlled flight + * where stick inputs map basically to the velocity setpoint + * + * @author Matthias Grob + */ + +#include "FlightTaskOrbit.hpp" + +using namespace matrix; + +int FlightTaskOrbit::activate() +{ + FlightTask::activate(); + r = 1.f; + v = 0.5f; + z = _position(2); + _center = Vector2f(_position.data()); + _center(0) -= r; + return 0; +} + +int FlightTaskOrbit::disable() +{ + FlightTask::disable(); + return 0; +} + +int FlightTaskOrbit::update() +{ + int ret = FlightTaskManual::update(); + + r += _sticks(0) * _deltatime; + r = math::constrain(r, 1.f, 20.f); + v -= _sticks(1) * _deltatime; + v = math::constrain(v, -7.f, 7.f); + z += _sticks(2) * _deltatime; + + Vector2f center_to_position = Vector2f(_position.data()) - _center; + + /* xy velocity to go around in a circle */ + Vector2f velocity_xy = Vector2f(center_to_position(1), -center_to_position(0)); + velocity_xy = velocity_xy.unit_or_zero(); + velocity_xy *= v; + + /* xy velocity adjustment to stay on the radius distance */ + velocity_xy += (r - center_to_position.norm()) * center_to_position.unit_or_zero(); + + float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F; + + _set_position_setpoint(Vector3f(NAN, NAN, z)); + _set_velocity_setpoint(Vector3f(velocity_xy(0), velocity_xy(1), 0.f)); + _set_yaw_setpoint(yaw); + return ret; +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 81e77b385b..21d7d67745 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -51,63 +51,6 @@ public: {}; virtual ~FlightTaskOrbit() {}; - /** - * Call once on the event where you switch to the task - * @return 0 on success, >0 on error otherwise - */ - virtual int activate() - { - FlightTask::activate(); - r = 1.f; - v = 0.5f; - z = _position(2); - _center = matrix::Vector2f(_position.data()); - _center(0) -= r; - return 0; - }; - - /** - * Call once on the event of switching away from the task - * @return 0 on success, >0 on error otherwise - */ - virtual int disable() - { - FlightTask::disable(); - return 0; - }; - - /** - * Call regularly in the control loop cycle to execute the task - * @return 0 on success, >0 on error otherwise - */ - virtual int update() - { - int ret = FlightTaskManual::update(); - - r += _sticks(0) * _deltatime; - r = math::constrain(r, 1.f, 20.f); - v -= _sticks(1) * _deltatime; - v = math::constrain(v, -7.f, 7.f); - z += _sticks(2) * _deltatime; - - matrix::Vector2 center_to_position = matrix::Vector2f(_position.data()) - _center; - - /* xy velocity to go around in a circle */ - matrix::Vector2 velocity_xy = matrix::Vector2f(center_to_position(1), -center_to_position(0)); - velocity_xy = velocity_xy.unit_or_zero(); - velocity_xy *= v; - - /* xy velocity adjustment to stay on the radius distance */ - velocity_xy += (r - center_to_position.norm()) * center_to_position.unit_or_zero(); - - float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F; - - _set_position_setpoint(matrix::Vector3f(NAN, NAN, z)); - _set_velocity_setpoint(matrix::Vector3f(velocity_xy(0), velocity_xy(1), 0.f)); - _set_yaw_setpoint(yaw); - return ret; - }; - private: float r = 0.f; /* radius with which to orbit the target */