From e5b2c652e22d174ea4f3b2f3ddd5c5877e359a33 Mon Sep 17 00:00:00 2001
From: Youssef Demitri <youssefyassa@gmail.com>
Date: Wed, 25 Nov 2015 10:28:19 +0100
Subject: [PATCH] channel airspeed meas over control state for q estimator

---
 .../attitude_estimator_q_main.cpp               | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 4b4fba54c8..f6037564e7 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -59,6 +59,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/att_pos_mocap.h>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -122,6 +123,7 @@ private:
 	int		_params_sub = -1;
 	int		_vision_sub = -1;
 	int		_mocap_sub = -1;
+	int		_airspeed_sub = -1;
 	int		_global_pos_sub = -1;
 	orb_advert_t	_att_pub = nullptr;
 	orb_advert_t	_ctrl_state_pub = nullptr;
@@ -161,6 +163,8 @@ private:
 	att_pos_mocap_s _mocap = {};
 	Vector<3>	_mocap_hdg;
 
+	airspeed_s _airspeed = {};
+
 	Quaternion	_q;
 	Vector<3>	_rates;
 	Vector<3>	_gyro_bias;
@@ -292,6 +296,8 @@ void AttitudeEstimatorQ::task_main()
 	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
 
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
@@ -473,6 +479,14 @@ void AttitudeEstimatorQ::task_main()
 			_mocap_hdg = Rmoc.transposed() * v;
 		}
 
+		// Update airspeed
+		bool airspeed_updated = false;
+		orb_check(_airspeed_sub, &airspeed_updated);
+
+		if (airspeed_updated) {
+			orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		}
+
 		// Check for timeouts on data
 		if (_ext_hdg_mode == 1) {
 			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
@@ -586,6 +600,9 @@ void AttitudeEstimatorQ::task_main()
 
 		ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
 
+		/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+		ctrl_state.airspeed = _airspeed.true_airspeed_m_s;
+
 		/* Publish to control state topic */
 		if (_ctrl_state_pub == nullptr) {
 			_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);