fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 05:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix get frame aligning quaternion function

Browse Source
This commit is contained in:
kamilritz
2019-08-22 14:00:07 +02:00
committed by Paul Riseborough
parent 53eac6e94e
commit a2ff415fe4
3 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+1 -1
View File
@@ -256,7 +256,7 @@ public:
void get_ekf_soln_status(uint16_t *status); void get_ekf_soln_status(uint16_t *status);
// return the quaternion defining the rotation from the EKF to the External Vision reference frame // return the quaternion defining the rotation from the EKF to the External Vision reference frame
void get_ekf2ev_quaternion(float *quat); void get_ev2ekf_quaternion(float *quat);
// use the latest IMU data at the current time horizon. // use the latest IMU data at the current time horizon.
Quatf calculate_quaternion() const; Quatf calculate_quaternion() const;
EKF/ekf_helper.cpp
+5 -5
View File
@@ -1653,14 +1653,14 @@ void Ekf::resetExtVisRotMat()
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference _ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
} }
// return the quaternions for the rotation from the EKF to the External Vision system frame of reference // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
void Ekf::get_ekf2ev_quaternion(float *quat) void Ekf::get_ev2ekf_quaternion(float *quat)
{ {
Quatf quat_ekf2ev; Quatf quat_ev2ekf;
quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt); quat_ev2ekf.from_axis_angle(_ev_rot_vec_filt);
for (unsigned i = 0; i < 4; i++) { for (unsigned i = 0; i < 4; i++) {
quat[i] = quat_ekf2ev(i); quat[i] = quat_ev2ekf(i);
} }
} }
EKF/estimator_interface.h
+1 -1
View File
@@ -287,7 +287,7 @@ public:
const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; } const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
// return the quaternion defining the rotation from the EKF to the External Vision reference frame // return the quaternion defining the rotation from the EKF to the External Vision reference frame
virtual void get_ekf2ev_quaternion(float *quat) = 0; virtual void get_ev2ekf_quaternion(float *quat) = 0;
// get the velocity of the body frame origin in local NED earth frame // get the velocity of the body frame origin in local NED earth frame
void get_velocity(float *vel) void get_velocity(float *vel)