added INDI wind estiamte message to logging

msg/CMakeLists.txt

@@ -144,6 +144,7 @@ set(msg_files
 	soaring_controller_position_setpoint.msg
 	soaring_controller_position.msg
 	soaring_controller_status.msg
+	soaring_controller_wind.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg

msg/soaring_controller_wind.msg

@@ -0,0 +1,6 @@
+# SOARING CONTROLLER WIND ESTIMATE, USED FOR INDI CONTROL
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32[3] wind_estimate                            # WIND ESTIMATE IN ENU FRAME
+float32[3] wind_estimate_filtered                   # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -722,6 +722,21 @@ FixedwingPositionINDIControl::Run()
             _actuators_0_pub.publish(_actuators);
             //print_message(_actuators);
 
+            // =====================
+            // publish wind estimate
+            // =====================
+            //_soaring_controller_wind = {};
+            _soaring_controller_wind.timestamp = hrt_absolute_time();
+            _soaring_controller_wind.wind_estimate[0] = wind(0);
+            _soaring_controller_wind.wind_estimate[1] = wind(1);
+            _soaring_controller_wind.wind_estimate[2] = wind(2);
+            _soaring_controller_wind.wind_estimate_filtered[0] = _wind_estimate(0);
+            _soaring_controller_wind.wind_estimate_filtered[1] = _wind_estimate(1);
+            _soaring_controller_wind.wind_estimate_filtered[2] = _wind_estimate(2);
+            _soaring_controller_wind_pub.publish(_soaring_controller_wind);
+
+
+
             if (_counter==100) {
                 _counter = 0;
                 //PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time));

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -66,6 +66,7 @@
 #include <uORB/topics/soaring_controller_position_setpoint.h>
 #include <uORB/topics/soaring_controller_position.h>
 #include <uORB/topics/soaring_controller_status.h>
+#include <uORB/topics/soaring_controller_wind.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/debug_value.h>
 #include <uORB/topics/wind.h>
@@ -133,6 +134,7 @@ private:
 	uORB::Publication<soaring_controller_heartbeat_s>				_soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)};
 	uORB::Publication<soaring_controller_position_setpoint_s>		_soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)};
 	uORB::Publication<soaring_controller_position_s>				_soaring_controller_position_pub{ORB_ID(soaring_controller_position)};
+	uORB::Publication<soaring_controller_wind_s>					_soaring_controller_wind_pub{ORB_ID(soaring_controller_wind)};
 	uORB::Publication<offboard_control_mode_s>						_offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
 	uORB::Publication<debug_value_s>								_debug_value_pub{ORB_ID(debug_value)};
 
@@ -155,6 +157,7 @@ private:
 	soaring_controller_heartbeat_s	_soaring_controller_heartbeat{};	///< soaring controller hrt
 	soaring_controller_position_setpoint_s	_soaring_controller_position_setpoint{};	///< soaring controller pos setpoint
 	soaring_controller_position_s	_soaring_controller_position{};	///< soaring controller pos 
+	soaring_controller_wind_s	_soaring_controller_wind{};	///< soaring controller wind
 	debug_value_s				_debug_value{};			// slip angle
 
 	// parameter struct

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -393,7 +393,7 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f);
  * @increment 0.0001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f);
+PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.01f);
 
 /**
  * roll gain of K_ACT_DAMPING (actuator damping gain)

src/modules/logger/logged_topics.cpp

@@ -124,6 +124,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_actuator_setpoint", 20);
 	add_topic("soaring_controller_position", 50);
 	add_topic("soaring_controller_position_setpoint", 50);
+	add_topic("soaring_controller_wind", 50);
 	add_topic("debug_value", 50);
 
 	// multi topics