yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_control: schedule trims for airspeed and flap deployment (#8937)

Browse Source
This commit is contained in:
Thomas Stastny
2018-03-16 21:55:44 +01:00
committed by Daniel Agar
parent 6d445cf5a6
commit 52e5e0df14
3 changed files with 170 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -85,6 +85,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.trim_roll = param_find("TRIM_ROLL");
_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
_parameter_handles.dtrim_roll_vmin = param_find("FW_DTRIM_R_VMIN");
_parameter_handles.dtrim_pitch_vmin = param_find("FW_DTRIM_P_VMIN");
_parameter_handles.dtrim_yaw_vmin = param_find("FW_DTRIM_Y_VMIN");
_parameter_handles.dtrim_roll_vmax = param_find("FW_DTRIM_R_VMAX");
_parameter_handles.dtrim_pitch_vmax = param_find("FW_DTRIM_P_VMAX");
_parameter_handles.dtrim_yaw_vmax = param_find("FW_DTRIM_Y_VMAX");
_parameter_handles.dtrim_roll_flaps = param_find("FW_DTRIM_R_FLPS");
_parameter_handles.dtrim_pitch_flaps = param_find("FW_DTRIM_P_FLPS");
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
@@ -163,6 +171,16 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.dtrim_roll_vmin, &(_parameters.dtrim_roll_vmin));
param_get(_parameter_handles.dtrim_roll_vmax, &(_parameters.dtrim_roll_vmax));
param_get(_parameter_handles.dtrim_pitch_vmin, &(_parameters.dtrim_pitch_vmin));
param_get(_parameter_handles.dtrim_pitch_vmax, &(_parameters.dtrim_pitch_vmax));
param_get(_parameter_handles.dtrim_yaw_vmin, &(_parameters.dtrim_yaw_vmin));
param_get(_parameter_handles.dtrim_yaw_vmax, &(_parameters.dtrim_yaw_vmax));
param_get(_parameter_handles.dtrim_roll_flaps, &(_parameters.dtrim_roll_flaps));
param_get(_parameter_handles.dtrim_pitch_flaps, &(_parameters.dtrim_pitch_flaps));
param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
@@ -651,6 +669,32 @@ void FixedwingAttitudeControl::run()
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
/* bi-linear interpolation over airspeed for actuator trim scheduling */
float trim_roll = _parameters.trim_roll;
float trim_pitch = _parameters.trim_pitch;
float trim_yaw = _parameters.trim_yaw;
if (airspeed < _parameters.airspeed_trim) {
trim_roll += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_roll_vmin,
0.0f);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_pitch_vmin,
0.0f);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_yaw_vmin,
0.0f);
} else {
trim_roll += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_roll_vmax);
trim_pitch += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_pitch_vmax);
trim_yaw += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f,
_parameters.dtrim_yaw_vmax);
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_applied * _parameters.dtrim_roll_flaps;
trim_pitch += _flaps_applied * _parameters.dtrim_pitch_flaps;
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
@@ -666,8 +710,7 @@ void FixedwingAttitudeControl::run()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
_parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
@@ -675,8 +718,7 @@ void FixedwingAttitudeControl::run()
}
float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
_parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
@@ -692,8 +734,7 @@ void FixedwingAttitudeControl::run()
yaw_u = _yaw_ctrl.control_euler_rate(control_input);
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
_parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
@@ -760,16 +801,13 @@ void FixedwingAttitudeControl::run()
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll :
_parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch :
_parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
_parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust) ? _rates_sp.thrust : 0.0f;
}

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

@@ -174,6 +174,14 @@ private:
float trim_roll;
float trim_pitch;
float trim_yaw;
float dtrim_roll_vmin;
float dtrim_pitch_vmin;
float dtrim_yaw_vmin;
float dtrim_roll_vmax;
float dtrim_pitch_vmax;
float dtrim_yaw_vmax;
float dtrim_roll_flaps;
float dtrim_pitch_flaps;
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
@@ -235,6 +243,14 @@ private:
param_t trim_roll;
param_t trim_pitch;
param_t trim_yaw;
param_t dtrim_roll_vmin;
param_t dtrim_pitch_vmin;
param_t dtrim_yaw_vmin;
param_t dtrim_roll_vmax;
param_t dtrim_pitch_vmax;
param_t dtrim_yaw_vmax;
param_t dtrim_roll_flaps;
param_t dtrim_pitch_flaps;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
param_t man_roll_max;

104
src/modules/fw_att_control/fw_att_control_params.c
View File

@@ -618,3 +618,107 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45);
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RATT_TH, 0.8f);
/**
* Roll trim increment at minimum airspeed
*
* This increment is added to TRIM_ROLL when airspeed is FW_AIRSPD_MIN.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMIN, 0.0f);
/**
* Pitch trim increment at minimum airspeed
*
* This increment is added to TRIM_PITCH when airspeed is FW_AIRSPD_MIN.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMIN, 0.0f);
/**
* Yaw trim increment at minimum airspeed
*
* This increment is added to TRIM_YAW when airspeed is FW_AIRSPD_MIN.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMIN, 0.0f);
/**
* Roll trim increment at maximum airspeed
*
* This increment is added to TRIM_ROLL when airspeed is FW_AIRSP_MAX.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_VMAX, 0.0f);
/**
* Pitch trim increment at maximum airspeed
*
* This increment is added to TRIM_PITCH when airspeed is FW_AIRSP_MAX.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
/**
* Yaw trim increment at maximum airspeed
*
* This increment is added to TRIM_YAW when airspeed is FW_AIRSP_MAX.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
/**
* Roll trim increment for flaps configuration
*
* This increment is added to TRIM_ROLL whenever flaps are fully deployed.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f);
/**
* Pitch trim increment for flaps configuration
*
* This increment is added to the pitch trim whenever flaps are fully deployed.
*
* @group FW Attitude Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f);