From ce3749d04fb11ec0407c0ae301e356df4845f9ea Mon Sep 17 00:00:00 2001 From: sander Date: Sat, 23 Jul 2016 22:52:24 +0200 Subject: [PATCH] Make tecs init airspeed mode dependent --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4107d17ae8..4894f4cbed 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -281,6 +281,7 @@ private: float airspeed_trim; float airspeed_max; float airspeed_trans; + int airspeed_mode; float pitch_limit_min; float pitch_limit_max; @@ -338,6 +339,7 @@ private: param_t airspeed_trim; param_t airspeed_max; param_t airspeed_trans; + param_t airspeed_mode; param_t pitch_limit_min; param_t pitch_limit_max; @@ -618,6 +620,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); + _parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE"); _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); @@ -703,6 +706,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans)); + param_get(_parameter_handles.airspeed_mode, &(_parameters.airspeed_mode)); param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); @@ -2339,8 +2343,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { _was_in_transition = true; // set this to transition airspeed to init tecs correctly - // some vtols fly without airspeed sensor - _asp_after_transition = _parameters.airspeed_trans; + if(_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED){ + // some vtols fly without airspeed sensor + _asp_after_transition = _parameters.airspeed_trans; + } else { + _asp_after_transition = _ctrl_state.airspeed; + } + _asp_after_transition = math::constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max); } else if (_was_in_transition) { // after transition we ramp up desired airspeed from the speed we had coming out of the transition