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

FixedWingPositionControl: rotate attitude consistenly for tailsitters

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2018-04-18 16:20:15 +02:00
committed by Lorenz Meier
parent e46e6f99f3
commit 0f1c9504d5
1 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+7 -8
View File
@@ -412,6 +412,13 @@ FixedwingPositionControl::vehicle_attitude_poll()
/* set rotation matrix and euler angles */
_R_nb = Quatf(_att.q);
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
_R_nb = _R_nb * R_offset;
}
Eulerf euler_angles(_R_nb);
_roll = euler_angles(0);
_pitch = euler_angles(1);
@@ -1878,14 +1885,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
/* Using tecs library */
float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad;
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
Eulerf euler = Eulerf(_R_nb * R_offset);
pitch_for_tecs = euler(1);
}
/* filter speed and altitude for controller */
Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);