fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make tecs init airspeed mode dependent

Browse Source
This commit is contained in:
sander 2016-07-23 22:52:24 +02:00 committed by Andreas Antener
parent 0cf77a30b1
commit ce3749d04f
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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