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

general fixes on VIO data access

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 7d7ee8e4bf
commit 8325724079
12 changed files with 56 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+3 -4
View File
@@ -599,7 +599,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
@@ -723,7 +723,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().vel_n = xLP(X_vx);
_pub_gpos.get().vel_e = xLP(X_vy);
_pub_gpos.get().vel_d = xLP(X_vz);
_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
@@ -827,8 +827,7 @@ void BlockLocalPositionEstimator::updateSSParams()
void BlockLocalPositionEstimator::predict()
{
// get acceleration
_q = matrix::Quatf(&_sub_att.get().q[0]);
_R_att = matrix::Dcm<float>(_q);
_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
-1
View File
@@ -436,7 +436,6 @@ private:
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
matrix::Quatf _q;
matrix::Dcm<float> _R_att;
Matrix<float, n_x, n_x> _A; // dynamics matrix
src/modules/local_position_estimator/sensors/mocap.cpp
+2 -2
View File
@@ -60,7 +60,7 @@ void BlockLocalPositionEstimator::mocapInit()
int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
// check if the mocap data is valid based on the covariances
_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6]));
_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
@@ -75,7 +75,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
if (!_mocap_xy_valid || !_mocap_z_valid) {
_time_last_mocap = _sub_mocap_odom.get().timestamp;
return !OK;
return -1;
} else {
y.setZero();
src/modules/local_position_estimator/sensors/vision.cpp
+2 -2
View File
@@ -65,7 +65,7 @@ void BlockLocalPositionEstimator::visionInit()
int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
{
if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
// check if the vision data is valid based on the covariances
_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6]));
_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
@@ -80,7 +80,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
if (!_vision_xy_valid || !_vision_z_valid) {
_time_last_vision_p = _sub_visual_odom.get().timestamp;
return !OK;
return -1;
} else {
y.setZero();