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;