zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

sbg
Thomas Stastny 7 years ago committed by Daniel Agar
parent
6d445cf5a6
commit
52e5e0df14
3 changed files with 170 additions and 12 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 62
      src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  2. 16
      src/modules/fw_att_control/FixedwingAttitudeControl.hpp
  3. 104
      src/modules/fw_att_control/fw_att_control_params.c

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

@ -85,6 +85,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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
Unescape View File

@ -174,6 +174,14 @@ private: @@ -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: @@ -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
Unescape View File

@ -618,3 +618,107 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); @@ -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);

Write Preview
Loading…
Cancel
Save