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

FixedWingAttitudeControl: use trim airspeed if airspeed is disabled

Browse Source 
- prior to this fix the fw attiude controller used airspeed to scale control
surfaces even though airspeed was disabled

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2018-04-03 09:34:27 +02:00 committed by Roman Bapst
parent 711b21cc5b
commit 8f9e8a6282
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -111,6 +111,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
_parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE");
// initialize to invalid VTOL type
_parameters.vtol_type = -1;
@ -130,6 +131,7 @@ int
FixedwingAttitudeControl::parameters_update()
{
int32_t tmp = 0;
param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
@ -153,9 +155,8 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff));
int32_t wheel_enabled = 0;
param_get(_parameter_handles.w_en, &wheel_enabled);
_parameters.w_en = (wheel_enabled == 1);
param_get(_parameter_handles.w_en, &tmp);
_parameters.w_en = (tmp == 1);
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
@ -210,6 +211,9 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
param_get(_parameter_handles.airspeed_mode, &tmp);
_parameters.airspeed_disabled = (tmp == 1);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.p_tc);
_pitch_ctrl.set_k_p(_parameters.p_p);
@ -565,13 +569,16 @@ void FixedwingAttitudeControl::run()
const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
&& (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
if (airspeed_valid) {
if (!_parameters.airspeed_disabled && airspeed_valid) {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
} else {
airspeed = _parameters.airspeed_trim;
perf_count(_nonfinite_input_perf);
if (!airspeed_valid) {
perf_count(_nonfinite_input_perf);
}
}
/*

2
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -202,6 +202,7 @@ private:
int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
int32_t bat_scale_en; /**< Battery scaling enabled */
bool airspeed_disabled;
} _parameters{}; /**< local copies of interesting parameters */
@ -269,6 +270,7 @@ private:
param_t vtol_type;
param_t bat_scale_en;
param_t airspeed_mode;
} _parameter_handles{}; /**< handles for interesting parameters */