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

Fix get frame aligning quaternion function

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/ekf.h
View File

@ -256,7 +256,7 @@ public:
void get_ekf_soln_status(uint16_t *status);
// 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.
Quatf calculate_quaternion() const;

10
EKF/ekf_helper.cpp
View File

@ -1653,14 +1653,14 @@ void Ekf::resetExtVisRotMat()
_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
void Ekf::get_ekf2ev_quaternion(float *quat)
// return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
void Ekf::get_ev2ekf_quaternion(float *quat)
{
Quatf quat_ekf2ev;
quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt);
Quatf quat_ev2ekf;
quat_ev2ekf.from_axis_angle(_ev_rot_vec_filt);
for (unsigned i = 0; i < 4; i++) {
quat[i] = quat_ekf2ev(i);
quat[i] = quat_ev2ekf(i);
}
}

2
EKF/estimator_interface.h
View File

@ -287,7 +287,7 @@ public:
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
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
void get_velocity(float *vel)