From 21d580293ad6a9908119a5f887486fa7ee2d69cf Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 24 Feb 2023 11:35:44 +0100 Subject: [PATCH] StickYaw: use consistently for all flight tasks - Switching to the first order filter that was previously only in FlightTaskManualAltitude. - Moving the scaling of full stick deflection to radians per second into the class. --- .../tasks/Auto/CMakeLists.txt | 2 +- .../tasks/Auto/FlightTaskAuto.cpp | 8 +++---- .../tasks/Auto/FlightTaskAuto.hpp | 7 +++---- .../FlightTaskManualAcceleration.cpp | 6 +++--- .../FlightTaskManualAcceleration.hpp | 4 ++-- .../FlightTaskManualAltitude.cpp | 18 +++------------- .../FlightTaskManualAltitude.hpp | 21 ++++++------------- .../tasks/Utility/CMakeLists.txt | 2 +- .../tasks/Utility/StickYaw.cpp | 18 +++++++--------- .../tasks/Utility/StickYaw.hpp | 18 ++++++++++------ 10 files changed, 42 insertions(+), 62 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 8d3ec4d7bc..2fadf2754d 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2023 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 diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index c1e4e096e8..5dffbdb6c7 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2023 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 @@ -243,9 +243,9 @@ void FlightTaskAuto::_prepareLandSetpoints() vertical_speed *= (1 + _sticks.getPositionExpo()(2)); // Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone - if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, - _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); + if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) { + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, + _deltatime); } _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 167b141f2a..5fb0b83ec0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2023 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 @@ -142,7 +142,7 @@ protected: Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; StickAccelerationXY _stick_acceleration_xy{this}; - StickYaw _stick_yaw; + StickYaw _stick_yaw{this}; matrix::Vector3f _land_position; float _land_heading; WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ @@ -176,8 +176,7 @@ protected: (ParamFloat) _param_mpc_z_v_auto_dn, (ParamFloat) _param_mpc_tko_speed, (ParamFloat) - _param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp - (ParamFloat) _param_mpc_man_y_max + _param_mpc_tko_ramp_t // time constant for smooth takeoff ramp ); private: diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index c1d1edaff4..2739a71e51 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -63,8 +63,8 @@ bool FlightTaskManualAcceleration::update() { bool ret = FlightTaskManualAltitudeSmoothVel::update(); - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, - _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, + _deltatime); _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position, diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 694bcb682f..d88f2d0bd6 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -58,7 +58,7 @@ private: void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; StickAccelerationXY _stick_acceleration_xy{this}; - StickYaw _stick_yaw; + StickYaw _stick_yaw{this}; WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index afd729152c..c4007b1a9a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2023 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 @@ -42,10 +42,6 @@ using namespace matrix; -FlightTaskManualAltitude::FlightTaskManualAltitude() : - _sticks(this) -{} - bool FlightTaskManualAltitude::updateInitialize() { bool ret = FlightTask::updateInitialize(); @@ -95,8 +91,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() void FlightTaskManualAltitude::_scaleSticks() { // Use stick input with deadzone, exponential curve and first order lpf for yawspeed - const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()); - _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, + _is_yaw_good_for_control, _deltatime); // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() : @@ -104,14 +100,6 @@ void FlightTaskManualAltitude::_scaleSticks() _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); } -float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target) -{ - const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f); - const float alpha = _deltatime / den; - _yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target; - return _yawspeed_filter_state; -} - void FlightTaskManualAltitude::_updateAltitudeLock() { // Depending on stick inputs and velocity, position is locked. diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 7f520798b6..4032625be4 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2023 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 @@ -40,14 +40,15 @@ #pragma once #include "FlightTask.hpp" -#include "Sticks.hpp" #include +#include "Sticks.hpp" +#include "StickYaw.hpp" #include class FlightTaskManualAltitude : public FlightTask { public: - FlightTaskManualAltitude(); + FlightTaskManualAltitude() = default; virtual ~FlightTaskManualAltitude() = default; bool activate(const trajectory_setpoint_s &last_setpoint) override; bool updateInitialize() override; @@ -73,7 +74,8 @@ protected: */ void _updateAltitudeLock(); - Sticks _sticks; + Sticks _sticks{this}; + StickYaw _stick_yaw{this}; bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, @@ -81,8 +83,6 @@ protected: (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_hold_max_xy, (ParamFloat) _param_mpc_z_p, /**< position controller altitude propotional gain */ - (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _param_mpc_man_y_tau, (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ (ParamFloat) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */ (ParamFloat) _param_mpc_land_alt2, /**< altitude below which to land with land speed */ @@ -97,14 +97,6 @@ private: void _unlockYaw(); void _lockYaw(); - /** - * Filter between stick input and yaw setpoint - * - * @param yawspeed_target yaw setpoint desired by the stick - * @return filtered value from independent filter state - */ - float _applyYawspeedFilter(float yawspeed_target); - /** * Terrain following. * During terrain following, the position setpoint is adjusted @@ -132,7 +124,6 @@ private: void setGearAccordingToSwitch(); - float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index cb30b9ea36..62b8e12179 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -38,5 +38,5 @@ px4_add_library(FlightTaskUtility StickYaw.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index bddb48c09c..8a2512279c 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -31,23 +31,19 @@ * ****************************************************************************/ -/** - * @file StickYaw.cpp - */ - #include "StickYaw.hpp" #include -StickYaw::StickYaw() -{ - _yawspeed_slew_rate.setSlewRate(2.f * M_PI_F); -} +StickYaw::StickYaw(ModuleParams *parent) : + ModuleParams(parent) +{} -void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, +void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, const float yaw, const bool is_yaw_good_for_control, const float deltatime) { - yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime); + _yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get()); + yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get())); yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control); } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index 8e666b551d..8e9e4aea1e 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -39,19 +39,20 @@ #pragma once -#include "SlewRate.hpp" +#include +#include -class StickYaw +class StickYaw : public ModuleParams { public: - StickYaw(); + StickYaw(ModuleParams *parent); ~StickYaw() = default; - void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float desired_yawspeed, float yaw, + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, bool is_yaw_good_for_control, float deltatime); private: - SlewRate _yawspeed_slew_rate; + AlphaFilter _yawspeed_filter; /** * Lock yaw when not currently turning @@ -65,4 +66,9 @@ private: * @return yaw setpoint to execute to have a yaw lock at the correct moment in time */ static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control); + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection + (ParamFloat) _param_mpc_man_y_tau ///< time constant for yaw speed filtering + ) };