fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

StickYaw: yaw rate limit interface

Browse Source
This commit is contained in:
Matthias Grob 2023-08-14 16:41:13 +02:00
parent 84220407ea
commit dbbf585adb
2 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp
View File

@ -66,7 +66,8 @@ void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint
}
_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()));
const float yawspeed_scale = math::min(math::radians(_param_mpc_man_y_max.get()), _yawspeed_constraint);
yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * yawspeed_scale);
yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev);
}

3
src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp
View File

@ -52,6 +52,7 @@ public:
void ekfResetHandler(float delta_yaw);
void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime,
float unaided_yaw = NAN);
void setYawspeedConstraint(float yawspeed) { _yawspeed_constraint = yawspeed; };
private:
AlphaFilter<float> _yawspeed_filter;
@ -78,6 +79,8 @@ private:
*/
float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const;
float _yawspeed_constraint{INFINITY};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau ///< time constant for yaw speed filtering