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

Add velocity observations to external vision interface

Browse Source
This commit is contained in:
kamilritz
2019-09-24 07:48:46 +10:00
committed by Paul Riseborough
parent 62a1e07512
commit 149233a9ab
2 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+8 -4
View File
@@ -80,10 +80,12 @@ struct flow_message {
};
struct ext_vision_message {
Vector3f posNED; ///< measured NED position relative to the local origin (m)
Quatf quat; ///< measured quaternion orientation defining rotation from NED to body frame
Vector3f posNED; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f velNED; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
float velErr; ///< 1-Sigma velocity accuracy (m/sec)
float angErr; ///< 1-Sigma angular error (rad)
};
@@ -151,10 +153,12 @@ struct flowSample {
};
struct extVisionSample {
Vector3f posNED; ///< measured NED position relative to the local origin (m)
Quatf quat; ///< measured quaternion orientation defining rotation from NED to body frame
Vector3f posNED; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f velNED; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
float velErr; ///< 1-Sigma velocity accuracy (m/sec)
float angErr; ///< 1-Sigma angular error (rad)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
EKF/estimator_interface.cpp
+2
View File
@@ -442,9 +442,11 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
// copy required data
ev_sample_new.angErr = evdata->angErr;
ev_sample_new.posErr = evdata->posErr;
ev_sample_new.velErr = evdata->velErr;
ev_sample_new.hgtErr = evdata->hgtErr;
ev_sample_new.quat = evdata->quat;
ev_sample_new.posNED = evdata->posNED;
ev_sample_new.velNED = evdata->velNED;
// record time for comparison next measurement
_time_last_ext_vision = time_usec;