From 9120082b0c953955803fdde6e2ea66878f01ef69 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 15 Feb 2017 23:02:47 -0500
Subject: [PATCH] fw_att_control stabilized mode publish att_sp quat

---
 src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 84676a9b57..793fd14248 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -70,6 +70,9 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/uORB.h>
 
+using matrix::Eulerf;
+using matrix::Quatf;
+
 /**
  * Fixedwing attitude control app start / stop handling function
  *
@@ -980,6 +983,14 @@ FixedwingAttitudeControl::task_main()
 					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
 					_att_sp.yaw_body = 0.0f;
 					_att_sp.thrust = _manual.z;
+
+					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;
+
 					int instance;
 					orb_publish_auto(_attitude_setpoint_id, &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
 				}
-- 
GitLab