diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index a9b814243d..570546e178 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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; } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index ab4844ff02..4f955b55dc 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -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; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 31711664ac..b46ac00fb0 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -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);