Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fw_att_control: schedule trims for airspeed and flap deployment #8937

Merged
merged 1 commit into from
Mar 16, 2018
Merged

fw_att_control: schedule trims for airspeed and flap deployment #8937

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 50 additions & 12 deletions src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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);