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

standard vtol: correctly modify attitude for pusher assist

Browse Source 
- fix a bug where the wrong rotation order was used to compute the attitude
setpoint when using the pusher assist feature

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-12-08 07:32:53 +01:00 committed by Lorenz Meier
parent 6b08ba6272
commit 0c49abbef8
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/vtol_att_control/standard.cpp
View File

@ -358,13 +358,13 @@ void Standard::update_mc_state()
// calculate the desired pitch seen in the heading frame
// this value corresponds to the amount the vehicle would try to pitch forward
float pitch_forward = asinf(body_z_sp(0));
float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2));
// only allow pitching forward up to threshold, the rest of the desired
// forward acceleration will be compensated by the pusher
if (pitch_forward < -_params_standard.down_pitch_max) {
// desired roll angle in heading frame stays the same
float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));
float roll_new = -asinf(body_z_sp(1));
_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
* _v_att_sp->thrust * _params_standard.forward_thrust_scale;
@ -383,9 +383,9 @@ void Standard::update_mc_state()
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
float pitch = asinf(tilt_new(0));
float roll = -atan2f(tilt_new(1), tilt_new(2));
R_sp = matrix::Eulerf(roll, pitch, euler_sp(2));
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
matrix::Quatf q_sp(R_sp);
memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
}