From 7c766692c40e6e26048a9e4ea529606b8cbbd90c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 11 Jan 2023 14:06:06 +0100 Subject: [PATCH] FW Att and Rate Controller, Tailsitter: fix tailsitter frame transformations Strictly follow the following convention for tailsitter: FW Attitude and FW rate controller always operate in the FW frame, meaning that roll is roll in FW, which for tailsitter means around the yaw axis in the body frame. The interfaces between modules is though always in body frame. That enables us to do the axis transformations for tailsitter, that are currently distributed all over the controller (attitude, rate, vtol module), only at the input and output data of modules. Side effect is that the FW rate control tuning gains meanings change: while before the roll gains where meant for the body axis, they are now always applied for the FW roll axis (also in hover). So the naming now is correct for FW, while before it was for Hover. Signed-off-by: Silvan Fuhrer --- .../FixedwingAttitudeControl.cpp | 11 ++-- .../fw_rate_control/FixedwingRateControl.cpp | 66 ++++++++++++++----- src/modules/vtol_att_control/tailsitter.cpp | 4 +- 3 files changed, 57 insertions(+), 24 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 7f7fb5abf4..b57df7962b 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -217,7 +217,6 @@ void FixedwingAttitudeControl::Run() vehicle_angular_velocity_s angular_velocity{}; _vehicle_rates_sub.copy(&angular_velocity); - float yawspeed = angular_velocity.xyz[2]; // only used for wheel controller if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode @@ -255,9 +254,6 @@ void FixedwingAttitudeControl::Run() /* fill in new attitude data */ _R = R_adapted; - - /* lastly, roll- and yawspeed have to be swaped */ - yawspeed = angular_velocity.xyz[0]; } const matrix::Eulerf euler_angles(_R); @@ -335,7 +331,7 @@ void FixedwingAttitudeControl::Run() control_input.roll = euler_angles.phi(); control_input.pitch = euler_angles.theta(); control_input.yaw = euler_angles.psi(); - control_input.body_z_rate = yawspeed; + control_input.body_z_rate = angular_velocity.xyz[2]; control_input.roll_setpoint = _att_sp.roll_body; control_input.pitch_setpoint = _att_sp.pitch_body; control_input.yaw_setpoint = _att_sp.yaw_body; @@ -385,6 +381,11 @@ void FixedwingAttitudeControl::Run() -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); } + // Tailsitter: transform from FW to hover frame (all interfaces are in hover (body) frame) + if (_vehicle_status.is_vtol_tailsitter) { + body_rates_setpoint = Vector3f(body_rates_setpoint(2), body_rates_setpoint(1), -body_rates_setpoint(0)); + } + /* Publish the rate setpoint for analysis once available */ _rates_sp.roll = body_rates_setpoint(0); _rates_sp.pitch = body_rates_setpoint(1); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index e3cf88a84c..c66a54d4c3 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -104,22 +104,42 @@ FixedwingRateControl::vehicle_manual_poll() !_vcontrol_mode.flag_control_attitude_enabled) { // RATE mode we need to generate the rate setpoint from manual user inputs + + if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // the rate_sp must always be published in body (hover) frame + _rates_sp.roll = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); + _rates_sp.yaw = -_manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); + + } else { + _rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); + _rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); + } + _rates_sp.timestamp = hrt_absolute_time(); - _rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get()); _rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get()); - _rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get()); _rates_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; _rate_sp_pub.publish(_rates_sp); } else { /* manual/direct control */ - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = - _manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + + if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + // the controls must always be published in body (hover) frame + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = + -(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get()); + + } else { + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + _manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get(); + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); + } + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = - _manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f; } } @@ -224,21 +244,17 @@ void FixedwingRateControl::Run() vehicle_angular_velocity_s angular_velocity{}; _vehicle_angular_velocity_sub.copy(&angular_velocity); - float rollspeed = angular_velocity.xyz[0]; - float pitchspeed = angular_velocity.xyz[1]; - float yawspeed = angular_velocity.xyz[2]; - const Vector3f rates(rollspeed, pitchspeed, yawspeed); - const Vector3f angular_accel{angular_velocity.xyz_derivative}; + Vector3f rates(angular_velocity.xyz); + Vector3f angular_accel{angular_velocity.xyz_derivative}; + + // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) if (_vehicle_status.is_vtol_tailsitter) { - - /* roll- and yawspeed have to be swaped */ - float helper = rollspeed; - rollspeed = -yawspeed; - yawspeed = helper; + rates = Vector3f(-angular_velocity.xyz[2], angular_velocity.xyz[1], angular_velocity.xyz[0]); + angular_accel = Vector3f(-angular_velocity.xyz_derivative[2], angular_velocity.xyz_derivative[1], + angular_velocity.xyz_derivative[0]); } - // this is only to pass through flaps/spoiler setpoints, can be removed once flaps/spoilers // are handled outside of attitude/rate controller. // TODO remove it @@ -341,7 +357,12 @@ void FixedwingRateControl::Run() if (_vcontrol_mode.flag_control_rates_enabled) { _rates_sp_sub.update(&_rates_sp); - const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + + // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) + if (_vehicle_status.is_vtol_tailsitter) { + body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); + } /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, @@ -404,6 +425,15 @@ void FixedwingRateControl::Run() _actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get() * constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); + // Tailsitter: rotate back to body frame from airspeed frame + if (_vehicle_status.is_vtol_tailsitter) { + const float helper = _actuator_controls.control[actuator_controls_s::INDEX_ROLL]; + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = + _actuator_controls.control[actuator_controls_s::INDEX_YAW]; + + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = -helper; + } + _actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); _actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); _actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index acfd4941e0..b82e24dd61 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -359,8 +359,10 @@ void Tailsitter::fill_actuator_outputs() } else { fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + + _torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL]; _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[2] = -fw_in[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW]; } _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;