diff --git a/src/lib/flight_tasks/tasks/Manual/CMakeLists.txt b/src/lib/flight_tasks/tasks/Manual/CMakeLists.txt index d652960d94..1c50ebc1f9 100644 --- a/src/lib/flight_tasks/tasks/Manual/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Manual/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskManual FlightTaskManual.cpp ) -target_link_libraries(FlightTaskManual PUBLIC FlightTask) +target_link_libraries(FlightTaskManual PUBLIC FlightTask FlightTaskUtility) target_include_directories(FlightTaskManual PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp index 89a2408871..6105c4e58c 100644 --- a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp +++ b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp @@ -42,13 +42,15 @@ using namespace matrix; using namespace time_literals; +FlightTaskManual::FlightTaskManual() : + _sticks(this) +{}; + bool FlightTaskManual::updateInitialize() { bool ret = FlightTask::updateInitialize(); - _sub_manual_control_setpoint.update(); - - const bool sticks_available = _evaluateSticks(); + const bool sticks_available = _sticks.evaluateSticks(_time_stamp_current, _gear); if (_sticks_data_required) { ret = ret && sticks_available; @@ -56,61 +58,3 @@ bool FlightTaskManual::updateInitialize() return ret; } - -bool FlightTaskManual::_evaluateSticks() -{ - hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s; - - /* Sticks are rescaled linearly and exponentially to [-1,1] */ - if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) { - - /* Linear scale */ - _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] */ - - /* Exponential scale */ - _sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); - _sticks_expo(1) = math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); - _sticks_expo(2) = math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get()); - _sticks_expo(3) = math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get()); - - // Only switch the landing gear up if the user switched from gear down to gear up. - // If the user had the switch in the gear up position and took off ignore it - // until he toggles the switch to avoid retracting the gear immediately on takeoff. - int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch; - - if (_gear_switch_old != gear_switch) { - _applyGearSwitch(gear_switch); - } - - _gear_switch_old = gear_switch; - - // valid stick inputs are required - const bool valid_sticks = PX4_ISFINITE(_sticks(0)) - && PX4_ISFINITE(_sticks(1)) - && PX4_ISFINITE(_sticks(2)) - && PX4_ISFINITE(_sticks(3)); - - return valid_sticks; - - } else { - /* Timeout: set all sticks to zero */ - _sticks.zero(); - _sticks_expo.zero(); - _gear.landing_gear = landing_gear_s::GEAR_KEEP; - return false; - } -} - -void FlightTaskManual::_applyGearSwitch(uint8_t gswitch) -{ - if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) { - _gear.landing_gear = landing_gear_s::GEAR_DOWN; - } - - if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) { - _gear.landing_gear = landing_gear_s::GEAR_UP; - } -} diff --git a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp index c0ad8dab88..074ad0436e 100644 --- a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp +++ b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp @@ -41,42 +41,16 @@ #pragma once #include "FlightTask.hpp" -#include +#include "Sticks.hpp" class FlightTaskManual : public FlightTask { public: - FlightTaskManual() = default; - + FlightTaskManual(); virtual ~FlightTaskManual() = default; - - bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); }; bool updateInitialize() override; protected: - + Sticks _sticks; bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */ - matrix::Vector _sticks; /**< unmodified manual stick inputs */ - matrix::Vector _sticks_expo; /**< modified manual sticks using expo function*/ - int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/ - - float stickDeadzone() const { return _param_mpc_hold_dz.get(); } - -private: - - bool _evaluateSticks(); /**< checks and sets stick inputs */ - void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */ - - uORB::SubscriptionData _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)}; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */ - (ParamFloat) - _param_mpc_xy_man_expo, /**< ratio of exponential curve for stick input in xy direction */ - (ParamFloat) - _param_mpc_z_man_expo, /**< ratio of exponential curve for stick input in z direction */ - (ParamFloat) - _param_mpc_yaw_expo, /**< ratio of exponential curve for stick input in yaw for modes except acro */ - (ParamFloat) _param_com_rc_loss_t /**< time at which commander considers RC lost */ - ) }; diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index c992e8cd5a..ff62f305df 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -88,12 +88,12 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() void FlightTaskManualAltitude::_scaleSticks() { // Use stick input with deadzone, exponential curve and first order lpf for yawspeed - const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get()); + const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()); _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target); // Use sticks input with deadzone and exponential curve for vertical velocity - const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; - _velocity_setpoint(2) = vel_max_z * _sticks_expo(2); + const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; + _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); } float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target) @@ -110,7 +110,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // If not locked, altitude setpoint is set to NAN. // Check if user wants to break - const bool apply_brake = fabsf(_sticks_expo(2)) <= FLT_EPSILON; + const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON; // Check if vehicle has stopped const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get()); @@ -122,7 +122,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() float spd_xy = Vector2f(_velocity).length(); // Use presence of horizontal stick inputs as a transition criteria - float stick_xy = Vector2f(&_sticks_expo(0)).length(); + float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length(); bool stick_input = stick_xy > 0.001f; if (_terrain_hold) { @@ -340,7 +340,7 @@ void FlightTaskManualAltitude::_updateSetpoints() // thrust along xy is demanded. The maximum thrust along xy depends on the thrust // setpoint along z-direction, which is computed in PositionControl.cpp. - Vector2f sp(&_sticks(0)); + Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0)); _man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get()); _man_input_filter.update(sp); @@ -359,8 +359,8 @@ void FlightTaskManualAltitude::_updateSetpoints() bool FlightTaskManualAltitude::_checkTakeoff() { - // stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1]) - return _sticks(2) < -0.3f; + // stick is deflected above 65% throttle (throttle stick is in the range [-1,1]) + return _sticks.getPosition()(2) < -0.3f; } bool FlightTaskManualAltitude::update() diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp index 6c00b3bf5b..4fc431f1aa 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp @@ -40,7 +40,7 @@ using namespace matrix; FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() : - _smoothing(this, _velocity(2), _sticks(2)) + _smoothing(this, _velocity(2), _sticks.getPosition()(2)) {} void FlightTaskManualAltitudeSmooth::_updateSetpoints() diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index a771e8aa58..b447ed9c1e 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -82,7 +82,7 @@ void FlightTaskManualPosition::_scaleSticks() FlightTaskManualAltitude::_scaleSticks(); /* Constrain length of stick inputs to 1 for xy*/ - Vector2f stick_xy = _sticks_expo.slice<2, 1>(0, 0); + Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0); const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp b/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp index 053359f43b..2782e3421d 100644 --- a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp +++ b/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp @@ -41,7 +41,7 @@ using namespace matrix; FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() : _smoothingXY(this, Vector2f(_velocity)), - _smoothingZ(this, _velocity(2), _sticks(2)) + _smoothingZ(this, _velocity(2), _sticks.getPosition()(2)) {} void FlightTaskManualPositionSmooth::_updateSetpoints() diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 55e58e03c6..b2a7d8365b 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -173,8 +173,8 @@ bool FlightTaskOrbit::update() bool ret = FlightTaskManualAltitudeSmooth::update(); // stick input adjusts parameters within a fixed time frame - const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f); - const float v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f); + const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f); + const float v = _v - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f); setRadius(r); setVelocity(v); @@ -257,7 +257,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position) case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED: _yaw_setpoint = NAN; - _yawspeed_setpoint = _sticks_expo(3); + _yawspeed_setpoint = _sticks.getPositionExpo()(3); break; case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER: diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt index 96dc2e818d..4a4de910a0 100644 --- a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_library(FlightTaskUtility VelocitySmoothing.cpp ManualVelocitySmoothingXY.cpp ManualVelocitySmoothingZ.cpp + Sticks.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier) diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp new file mode 100644 index 0000000000..df368bf6a1 --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 Sticks.cpp + */ + +#include "Sticks.hpp" + +using namespace time_literals; + +Sticks::Sticks(ModuleParams *parent) : + ModuleParams(parent) +{}; + +bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear) +{ + _sub_manual_control_setpoint.update(); + + hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s; + + // Sticks are rescaled linearly and exponentially to [-1,1] + if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) { + + // Linear scale + _positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1] + _positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1] + _positions(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1] + _positions(3) = _sub_manual_control_setpoint.get().r; // yaw [-1,1] + + // Exponential scale + _positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); + _positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); + _positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get()); + _positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get()); + + // Only switch the landing gear up if the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch; + + if (_gear_switch_old != gear_switch) { + applyGearSwitch(gear_switch, gear); + } + + _gear_switch_old = gear_switch; + + // valid stick inputs are required + const bool valid_sticks = PX4_ISFINITE(_positions(0)) + && PX4_ISFINITE(_positions(1)) + && PX4_ISFINITE(_positions(2)) + && PX4_ISFINITE(_positions(3)); + + return valid_sticks; + + } else { + // Timeout: set all sticks to zero + _positions.zero(); + _positions_expo.zero(); + gear.landing_gear = landing_gear_s::GEAR_KEEP; + return false; + } +} + +void Sticks::applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear) +{ + if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + gear.landing_gear = landing_gear_s::GEAR_DOWN; + } + + if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + gear.landing_gear = landing_gear_s::GEAR_UP; + } +} diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp new file mode 100644 index 0000000000..28a878e55b --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 Sticks.hpp + * + * Library abstracting stick input from manual_control_setpoint + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include +#include + +class Sticks : public ModuleParams +{ +public: + Sticks(ModuleParams *parent); + ~Sticks() = default; + + bool evaluateSticks(hrt_abstime now, landing_gear_s &gear); ///< checks and sets stick inputs + void applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear); ///< Sets gears according to switch + const matrix::Vector &getPosition() { return _positions; }; + const matrix::Vector &getPositionExpo() { return _positions_expo; }; +private: + matrix::Vector _positions; ///< unmodified manual stick inputs + matrix::Vector _positions_expo; ///< modified manual sticks using expo function + int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state + + uORB::SubscriptionData _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_hold_dz, + (ParamFloat) _param_mpc_xy_man_expo, + (ParamFloat) _param_mpc_z_man_expo, + (ParamFloat) _param_mpc_yaw_expo, + (ParamFloat) _param_com_rc_loss_t + ) +};