Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Modified comment about feed forward of yaw setpoint rate.

Browse Source 
Added more explanations regarding the coordinate transformation.

Signed-off-by: Ivo Drescher <ivo.drescher@gmail.com>
This commit is contained in:
Ivo Drescher
2018-06-13 10:37:35 +02:00
committed by Matthias Grob
parent b583c5f69a
commit 77d4554e6d
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+8 -3
View File
@@ -417,9 +417,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* calculate angular rates setpoint */ /* calculate angular rates setpoint */
_rates_sp = eq.emult(attitude_gain); _rates_sp = eq.emult(attitude_gain);
/* Feed forward the yaw setpoint rate. We need to apply the yaw rate in the body frame. /* Feed forward the yaw setpoint rate.
* We infer the body z axis by taking the last column of R.transposed (== q.inversed) * The yaw_feedforward_rate is a commanded rotation around the world z-axis,
* because it's the rotation axis for body yaw and multiply it by the rate and gain. */ * but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
* Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
* and multiply it by the yaw setpoint rate (yaw_sp_move_rate) and gain (_yaw_ff).
* 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.
*/
Vector3f yaw_feedforward_rate = q.inversed().dcm_z(); Vector3f yaw_feedforward_rate = q.inversed().dcm_z();
yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _yaw_ff.get(); yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _yaw_ff.get();
_rates_sp += yaw_feedforward_rate; _rates_sp += yaw_feedforward_rate;