yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00: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
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 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 #define ALTHOLD_EPV_RESET_THRESH 5.0f
using matrix::Eulerf;
using matrix::Quatf;
/** /**
* L1 control app start / stop handling function * 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); _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 */ /* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) { if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */ /* publish the attitude setpoint */