From 94d6a92f41515dab84809dbf4ba5cb4ed6d13305 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 6 Feb 2017 22:16:07 -0500 Subject: [PATCH] fw_pos_control_l1 publish quat in att_sp --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fcdcf795a0..6be83db41c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -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 */