diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index a3eef9d3a1..d3cb480a15 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit a3eef9d3a17078896f40abb0deaabfb0faad8c65 +Subproject commit d3cb480a15d64a153513cd9cb68abd8bb5f4c1f8 diff --git a/msg/soaring_estimator_shear.msg b/msg/soaring_estimator_shear.msg index 27afc8709f..afb2b9cf5b 100644 --- a/msg/soaring_estimator_shear.msg +++ b/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 diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 1cc6f7178b..dbba029962 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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(); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 848a378f1a..822a821c10 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -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 _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) _param_shear_height, // thrust params (ParamFloat) _param_thrust, - // RC feedthrough params - (ParamInt) _param_switch_manual, // force saturation (ParamInt) _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; // diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 3b09228281..09524cba1c 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/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 ============ // ====================================================== diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 4acf08f0a3..7eeb6d7659 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/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 a = _findClosest(_alpha_arr, 9, _shear_alpha); // shear strength + float shear = sqrtf(powf(_X_posterior_horizontal(0)*_unit_v - _X_posterior_horizontal(2)*_unit_v, 2) + + 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])