Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mc_att_control_main: separate yaw rate setpoint generation from absolute yaw reset

Browse Source
This commit is contained in:
Matthias Grob
2023-02-02 16:49:46 +01:00
committed by Silvan Fuhrer
parent ce5cff55b7
commit 2b0f7879bc
1 changed files with 10 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+10 -6
View File
@@ -112,15 +112,19 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
vehicle_attitude_setpoint_s attitude_setpoint{};
const float yaw = Eulerf(q).psi();
/* reset yaw setpoint to current position if needed */
// Avoid giving yaw rate setpoint with arming stick gesture
if ((_manual_control_setpoint.throttle > -.9f) || (_param_mc_airmode.get() == 2)) {
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_mpc_man_y_max.get());
} else {
attitude_setpoint.yaw_sp_move_rate = 0.f;
}
// Make sure not absolute heading error builds up
if (reset_yaw_sp) {
_man_yaw_sp = yaw;
} else if ((_manual_control_setpoint.throttle > -.9f)
|| (_param_mc_airmode.get() == 2)) {
const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate;
} else {
_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
}