From e1b770c99c09072bd089aa8ea3e31ff650f305c5 Mon Sep 17 00:00:00 2001
From: Marvin Harms <harmsm@ethz.ch>
Date: Thu, 7 Apr 2022 10:02:53 +0200
Subject: [PATCH] initial commit

---
 .../FixedwingPositionINDIControl.cpp          | 40 ++++++++++++++++++-
 1 file changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 480945fe47..1abcb7fce3 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -44,4 +44,42 @@ using matrix::Quatf;
 using matrix::Vector2f;
 using matrix::Vector2d;
 using matrix::Vector3f;
-using matrix::wrap_pi;
\ No newline at end of file
+using matrix::wrap_pi;
+
+
+FixedwingPositionINDIControl::FixedwingPositionINDIControl() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
+{
+	// limit to 50 Hz
+	_local_pos_sub.set_interval_ms(20);
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+FixedwingPositionINDIControl::~FixedwingPositionINDIControl()
+{
+	perf_free(_loop_perf);
+}
+
+void
+FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
+{
+    _wind_estimate = wind;
+}
+
+Vector
+FixedwingPositionINDIControl::_get_basis_funs(float t)
+{
+    std::vector<float> vec = {1};
+    float sigma = 0.5/_num_basis_funs;
+    for(int i=1; i<_num_basis_funs; i++){
+        float fun1 = sinf(M_PI*t);
+        float fun2 = exp(-pow((t-i/_num_basis_funs),2)/sigma);
+        vec.push_back(fun1*fun2);
+    }
+    return Vector(vec);
+}
\ No newline at end of file