intermediate push

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);
 
 

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;
     }
 

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)

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