From 0a355253ffa134ee3679576c6836e876c24233a5 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 10 Jan 2024 11:41:53 +0100 Subject: [PATCH] fw: convert trim to vector3f --- .../fw_rate_control/FixedwingRateControl.cpp | 40 +++++++++---------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 37c2d98ae0..606f421166 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -322,28 +322,26 @@ void FixedwingRateControl::Run() } /* bi-linear interpolation over airspeed for actuator trim scheduling */ - float trim_roll = _param_trim_roll.get(); - float trim_pitch = _param_trim_pitch.get(); - float trim_yaw = _param_trim_yaw.get(); + Vector3f trim(_param_trim_roll.get(), _param_trim_pitch.get(), _param_trim_yaw.get()); if (airspeed < _param_fw_airspd_trim.get()) { - trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_r_vmin.get(), - 0.0f); - trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_p_vmin.get(), - 0.0f); - trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), - _param_fw_dtrim_y_vmin.get(), - 0.0f); + trim(0) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_r_vmin.get(), + 0.0f); + trim(1) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_p_vmin.get(), + 0.0f); + trim(2) += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), + _param_fw_dtrim_y_vmin.get(), + 0.0f); } else { - trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_r_vmax.get()); - trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_p_vmax.get()); - trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, - _param_fw_dtrim_y_vmax.get()); + trim(0) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_r_vmax.get()); + trim(1) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_p_vmax.get()); + trim(2) += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f, + _param_fw_dtrim_y_vmax.get()); } if (_vcontrol_mode.flag_control_rates_enabled) { @@ -382,9 +380,9 @@ void FixedwingRateControl::Run() _rate_control.resetIntegral(); } - _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; - _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; - _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; + _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim(0), -1.f, 1.f) : trim(0); + _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim(1), -1.f, 1.f) : trim(1); + _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim(2), -1.f, 1.f) : trim(2); /* throttle passed through if it is finite */ _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;