Skip to content

Commit

Permalink
fw_att_control: schedule trims for airspeed and flap deployment (#8937)
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
Thomas Stastny authored and dagar committed Mar 16, 2018
1 parent 6d445cf commit 52e5e0d
Show file tree
Hide file tree
Showing 3 changed files with 170 additions and 12 deletions.
62 changes: 50 additions & 12 deletions src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)) {
Expand All @@ -666,17 +710,15 @@ 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();
perf_count(_nonfinite_output_perf);
}

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();
Expand All @@ -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) {
Expand Down Expand Up @@ -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;
}
Expand Down
16 changes: 16 additions & 0 deletions src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,14 @@ class FixedwingAttitudeControl final : public control::SuperBlock, public Module
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 */
Expand Down Expand Up @@ -235,6 +243,14 @@ class FixedwingAttitudeControl final : public control::SuperBlock, public Module
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;
Expand Down
104 changes: 104 additions & 0 deletions src/modules/fw_att_control/fw_att_control_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

0 comments on commit 52e5e0d

Please sign in to comment.