Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+11 -4
View File
@@ -111,6 +111,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.rattitude_thres = param_find("FW_RATT_TH"); _parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN"); _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 // initialize to invalid VTOL type
_parameters.vtol_type = -1; _parameters.vtol_type = -1;
@@ -130,6 +131,7 @@ int
FixedwingAttitudeControl::parameters_update() FixedwingAttitudeControl::parameters_update()
{ {
int32_t tmp = 0;
param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_i, &(_parameters.p_i)); 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.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff)); param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff));
int32_t wheel_enabled = 0; param_get(_parameter_handles.w_en, &tmp);
param_get(_parameter_handles.w_en, &wheel_enabled); _parameters.w_en = (tmp == 1);
_parameters.w_en = (wheel_enabled == 1);
param_get(_parameter_handles.w_p, &(_parameters.w_p)); param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i)); 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.bat_scale_en, &_parameters.bat_scale_en);
param_get(_parameter_handles.airspeed_mode, &tmp);
_parameters.airspeed_disabled = (tmp == 1);
/* pitch control parameters */ /* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.p_tc); _pitch_ctrl.set_time_constant(_parameters.p_tc);
_pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_p(_parameters.p_p);
@@ -565,14 +569,17 @@ void FixedwingAttitudeControl::run()
const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s) const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
&& (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6); && (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 */ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s); airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
} else { } else {
airspeed = _parameters.airspeed_trim; airspeed = _parameters.airspeed_trim;
if (!airspeed_valid) {
perf_count(_nonfinite_input_perf); perf_count(_nonfinite_input_perf);
} }
}
/* /*
* For scaling our actuators using anything less than the min (close to stall) * For scaling our actuators using anything less than the min (close to stall)
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2
View File
@@ -202,6 +202,7 @@ private:
int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
int32_t bat_scale_en; /**< Battery scaling enabled */ int32_t bat_scale_en; /**< Battery scaling enabled */
bool airspeed_disabled;
} _parameters{}; /**< local copies of interesting parameters */ } _parameters{}; /**< local copies of interesting parameters */
@@ -269,6 +270,7 @@ private:
param_t vtol_type; param_t vtol_type;
param_t bat_scale_en; param_t bat_scale_en;
param_t airspeed_mode;
} _parameter_handles{}; /**< handles for interesting parameters */ } _parameter_handles{}; /**< handles for interesting parameters */