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

do attitude estimate rotation in EKf for tailsitter in FW

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-08 17:56:45 +02:00
parent 36af12a1d4
commit e637e63d21
3 changed files with 49 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+35
View File
@@ -532,6 +532,7 @@ void EKF2::Run()
_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
_is_tailsitter_in_fw = is_fixed_wing && vehicle_status.is_vtol_tailsitter;
}
}
@@ -667,6 +668,40 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
/* get current rotation matrix and euler angles from control state quaternions */
matrix::Dcmf R = matrix::Quatf(att.q);
if (_is_tailsitter_in_fw) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
* */
matrix::Dcmf R_adapted = R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
/* fill in new attitude data */
R = R_adapted;
const matrix::Eulerf euler_angles(R);
const matrix::Quatf quat_new(matrix::Eulerf(euler_angles.phi(), euler_angles.theta(), euler_angles.psi()));
quat_new.copyTo(att.q);
}
_attitude_pub.publish(att);
} else if (_replay_mode) {
src/modules/ekf2/EKF2.hpp
+2
View File
@@ -183,6 +183,8 @@ private:
}
}
bool _is_tailsitter_in_fw{false};
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+12 -12
View File
@@ -319,20 +319,20 @@ void FixedwingAttitudeControl::Run()
* */
matrix::Dcmf R_adapted = R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
// /* move z to x */
// R_adapted(0, 0) = R(0, 2);
// R_adapted(1, 0) = R(1, 2);
// R_adapted(2, 0) = R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
// /* move x to z */
// R_adapted(0, 2) = R(0, 0);
// R_adapted(1, 2) = R(1, 0);
// R_adapted(2, 2) = R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
// /* change direction of pitch (convert to right handed system) */
// R_adapted(0, 0) = -R_adapted(0, 0);
// R_adapted(1, 0) = -R_adapted(1, 0);
// R_adapted(2, 0) = -R_adapted(2, 0);
/* fill in new attitude data */
R = R_adapted;