Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 14:40:12 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

Handle invalid yaw rate setpoints

Browse Source
This commit is contained in:
Jaeyoung-Lim
2021-03-13 23:00:29 +01:00
committed by Lorenz Meier
parent 06e3d38bbd
commit 6b4ccaa53e
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
+2
View File
@@ -97,7 +97,9 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
// and multiply it by the yaw setpoint rate (yawspeed_setpoint). // and multiply it by the yaw setpoint rate (yawspeed_setpoint).
// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
// such that it can be added to the rates setpoint. // such that it can be added to the rates setpoint.
if (is_finite(_yawspeed_setpoint)) {
rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
}
// limit rates // limit rates
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {