fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 14:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Att and Rate Controller, Tailsitter: fix tailsitter frame transformations

Browse Source 
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-13 18:18:11 +01:00
committed by JaeyoungLim
parent edb59a9000
commit 7c766692c4
3 changed files with 57 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+6 -5
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);