From a2262d7ec4c555d5f4262d59fa9b84da47d8f652 Mon Sep 17 00:00:00 2001
From: Marvin Harms <harmsm@ethz.ch>
Date: Mon, 8 Aug 2022 18:10:20 +0200
Subject: [PATCH] intermediate push

---
 .../FixedwingPositionINDIControl.cpp                 |  3 +++
 .../FixedwingShearEstimator.cpp                      | 12 ++++++++----
 .../fw_dyn_soar_estimator_params.c                   |  6 +++---
 src/modules/logger/logged_topics.cpp                 |  1 +
 4 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 489578c325..42b56d57d8 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -872,6 +872,9 @@ FixedwingPositionINDIControl::Run()
             _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.position[0] = _pos(0);
+            _soaring_controller_wind.position[1] = _pos(1);
+            _soaring_controller_wind.position[2] = _pos(2);
             _soaring_controller_wind_pub.publish(_soaring_controller_wind);
 
 
diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
index fdf040c0b8..5f94853e2c 100644
--- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
@@ -53,7 +53,6 @@ FixedwingShearEstimator::FixedwingShearEstimator() :
 	// limit to 10 Hz
 	_soaring_controller_wind_sub.set_interval_ms(100.f);
 
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -101,6 +100,11 @@ FixedwingShearEstimator::init()
     // init reset counter
     _reset_counter = 0;
 
+    //
+    _X_prior_horizontal(4) = 110.f;
+    _X_posterior_horizontal(4) = 110.f;
+
+    return true;
 }
 
 int
@@ -154,7 +158,7 @@ void
 FixedwingShearEstimator::perform_prior_update()
 {
     // get time since last run
-    float dt = (hrt_absolute_time() - _last_run)/1000000.f;
+    float dt = (hrt_absolute_time() - _last_run)/1000000;
     _last_run = hrt_absolute_time();
 
     // perform prior update assuming trivial dynamics of the wind field (mean field stays the same)
@@ -203,7 +207,7 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
         _K_horizontal = tmp1_horizontal*inv_horizontal;
     }
     else{
-        PX4_ERR("singular horizontal matrix, resetting filter");
+        PX4_WARN("singular horizontal matrix, resetting filter");
         error = true;
     }
 
@@ -222,7 +226,7 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
         _K_vertical = tmp1_vertical*inv_vertical;
     }
     else{
-        PX4_ERR("singular vertical matrix, resetting filter");
+        PX4_WARN("singular vertical matrix, resetting filter");
         error = true;
     }
 
diff --git a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
index 1e3eb32aad..e9f399ef43 100644
--- a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
+++ b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c
@@ -22,7 +22,7 @@
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.0001f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.001f);
 
 /**
  * Standard deviation of vertical shear position
@@ -36,7 +36,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.0001f);
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.001f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.01f);
 
 /**
  * Standard deviation of velicity state in shear model
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.001f);
  * @increment 0.000001
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.00003f);
+PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.0003f);
 
 /**
  * Standard deviation of velicity measurement (wind)
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index a6c8af62d4..04eb79f7ea 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -125,6 +125,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("soaring_controller_position", 10);
 	add_topic("soaring_controller_position_setpoint", 10);
 	add_topic("soaring_controller_wind", 50);
+	add_topic("soaring_estimator_shear", 50);
 	add_topic("debug_value", 50);
 
 	// multi topics