removed param DS_SWITCH_MANUAL, added shear message

msg/soaring_estimator_shear.msg

@@ -20,5 +20,10 @@ float32 coeff_0             # offset vertical wind
 float32 coeff_1             # linear coeff vertical wind
 float32 coeff_2             # quadratic coeff vertical wind
 
+float32 v_max               # discrete value of v_max (shear velocity)
+float32 alpha               # discrete value of alpha (shear strength)
+float32 h_ref               # discrete value of h_ref (shear location)
+float32 psi                 # discrete value of shear heading
+
 bool soaring_feasible       # plausibility check
 uint64 reset_counter        # filter reset counter

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -179,8 +179,6 @@ FixedwingPositionINDIControl::parameters_update()
 
     _thrust = _param_thrust.get();
 
-    _switch_manual = _param_switch_manual.get();
-
     _switch_saturation = _param_switch_saturation.get();
 
     _switch_filter = _param_switch_filter.get();
@@ -288,6 +286,19 @@ FixedwingPositionINDIControl::manual_control_setpoint_poll()
     }
 }
 
+void
+FixedwingPositionINDIControl::rc_channels_poll()
+{
+    _rc_channels_sub.update(&_rc_channels);
+    // use flaps channel to select manual feedthrough
+    if (_rc_channels.channels[5]>=0.f){
+        _switch_manual = 1;
+    }
+    else{
+        _switch_manual = 0;
+    }
+}
+
 void
 FixedwingPositionINDIControl::vehicle_attitude_poll()
 {
@@ -713,6 +724,7 @@ FixedwingPositionINDIControl::Run()
         airspeed_poll();
         airflow_aoa_poll();
         airflow_slip_poll();
+        rc_channels_poll();
         manual_control_setpoint_poll();
         vehicle_local_position_poll();
         vehicle_attitude_poll();
@@ -1395,7 +1407,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
     // ====================================
     if (_switch_manual){
         // get an attitude setpoint from the current manual inputs
-        float roll_ref = 1.f * _manual_control_setpoint.y * M_PI_4_F;
+        float roll_ref = 1.f * _manual_control_setpoint.y * 1.0f;
         float pitch_ref = -1.f* _manual_control_setpoint.x * M_PI_4_F;
         Eulerf E_current(Quatf(_attitude.q));
         float yaw_ref = E_current.psi();

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/airflow_aoa.h>
 #include <uORB/topics/airflow_slip.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/rc_channels.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -126,6 +127,7 @@ private:
 	uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)};			// vehicle status flags
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)};
 	
     // Publishers
 	uORB::Publication<actuator_controls_s>							_actuators_0_pub;
@@ -144,6 +146,7 @@ private:
 	vehicle_angular_acceleration_setpoint_s _angular_accel_sp {};
 	actuator_controls_s			_actuators {};			// actuator commands
 	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
+	rc_channels_s				_rc_channels {};		///< rc channels
 	vehicle_local_position_s	_local_pos {};			///< vehicle local position
 	vehicle_acceleration_s		_acceleration {};		///< vehicle acceleration
 	vehicle_attitude_s			_attitude {};			///< vehicle attitude
@@ -213,8 +216,6 @@ private:
 		(ParamFloat<px4::params::DS_W_HEIGHT>) _param_shear_height,
 		// thrust params
 		(ParamFloat<px4::params::DS_THRUST>) _param_thrust,
-		// RC feedthrough params
-		(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual,
 		// force saturation
 		(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation,
 		// command filtering
@@ -248,6 +249,7 @@ private:
 	
 	void		control_update();
 	void 		manual_control_setpoint_poll();
+	void		rc_channels_poll();
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
 	void		vehicle_status_poll();
@@ -360,7 +362,7 @@ private:
 	// thrust
 	float _thrust;
 	// controller mode
-	bool _switch_manual;
+	bool _switch_manual = 1;
 	// force limit
 	bool _switch_saturation;
 	//

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -549,22 +549,6 @@ PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f);
  */
 PARAM_DEFINE_FLOAT(DS_THRUST, 0);
 
-// ======================================================
-// ================ Stick feedthorugh ===================
-// ======================================================
-
-/**
- * integer in {0,1} defining if manual attitude setpoints are commanded by the pilot, 0=DS-controller, 1=manual feedthrough
- * 
- * @unit 
- * @min 0
- * @max 1
- * @decimal 1
- * @increment 1
- * @group FW DYN SOAR Control
- */
-PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);
-
 // ======================================================
 // ============= controller force saturation ============
 // ======================================================

src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp

@@ -341,10 +341,16 @@ FixedwingShearEstimator::Run()
         }
 
         // get the correct shear params for trajectory selection
-        float v = _findClosest(_v_max_arr, 5, _shear_v_max);    // wind velocity
+        float shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v - _X_posterior_horizontal(2)*_unit_v, 2) + 
-        float a = _findClosest(_alpha_arr, 9, _shear_alpha);    // shear strength
+                            powf(_X_posterior_horizontal(1)*_unit_v - _X_posterior_horizontal(3)*_unit_v, 2));
+        float v = _findClosest(_v_max_arr, 5, shear);    // wind velocity
+        float a = _findClosest(_alpha_arr, 9, _X_posterior_horizontal(5)*_unit_a);    // shear strength
         float heading = -atan2f(_X_posterior_horizontal(1)-_X_posterior_horizontal(3),
                                 _X_posterior_horizontal(0)-_X_posterior_horizontal(2));
+        _soaring_estimator_shear.v_max = v;
+        _soaring_estimator_shear.alpha = a;
+        _soaring_estimator_shear.h_ref = _X_posterior_horizontal(4)*_unit_h;
+        _soaring_estimator_shear.psi = heading;
 
         // publish shear params
         // ========================================
@@ -408,7 +414,7 @@ bool FixedwingShearEstimator::check_feasibility()
 }
 
 float
-FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target)
+FixedwingShearEstimator::_getClosest(float val1, float val2, float target)
 {
     if (target - val1 >= val2 - target)
         return val2;
@@ -417,7 +423,7 @@ FixedwingPositionINDIControl::_getClosest(float val1, float val2, float target)
 }
 
 float
-FixedwingPositionINDIControl::_findClosest(float arr[], int n, float target)
+FixedwingShearEstimator::_findClosest(float arr[], int n, float target)
 {
     // Corner cases
     if (target <= arr[0])