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

fw_pos_control_l1 publish quat in att_sp

Browse Source
This commit is contained in:
Daniel Agar 2017-02-06 22:16:07 -05:00 committed by Lorenz Meier
parent feda5caac2
commit 94d6a92f41
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -115,6 +115,9 @@ static int _control_task = -1; /**< task handle for sensor task */
#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f ///< a throttle / pitch input above this value leads to the system switching to climbout mode
#define ALTHOLD_EPV_RESET_THRESH 5.0f
using matrix::Eulerf;
using matrix::Quatf;
/**
* L1 control app start / stop handling function
*
@ -2339,6 +2342,13 @@ FixedwingPositionControl::task_main()
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad);
}
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
_att_sp.q_d[0] = q(0);
_att_sp.q_d[1] = q(1);
_att_sp.q_d[2] = q(2);
_att_sp.q_d[3] = q(3);
_att_sp.q_d_valid = true;
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */