dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-01-11 14:06:06 +01:00 committed by JaeyoungLim
parent edb59a9000
commit 7c766692c4
3 changed files with 57 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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);

66
src/modules/fw_rate_control/FixedwingRateControl.cpp
View File

@ -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;

4
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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;