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

Update setExtVisionData interface

Browse Source
This commit is contained in:
kamilritz
2020-01-23 14:50:43 +01:00
committed by Mathieu Bresciani
parent ed916c8006
commit de33885658
5 changed files with 16 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
-9
View File
@@ -80,15 +80,6 @@ struct flow_message {
uint32_t dt; ///< integration time of flow samples (microseconds)
};
struct ext_vision_message {
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
Vector3f posVar; ///< XYZ position variances (m**2)
Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2)
float angVar; ///< angular heading variance (rad**2)
};
struct outputSample {
Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude
Vector3f vel; ///< NED velocity estimate in earth frame (m/sec)
EKF/estimator_interface.cpp
+7 -13
View File
@@ -403,7 +403,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
}
// set attitude and position data derived from an external vision system
void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
void EstimatorInterface::setExtVisionData(const extVisionSample& evdata)
{
if (!_initialised || _ev_buffer_fail) {
return;
@@ -421,19 +421,13 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
}
// limit data rate to prevent data being lost
if ((time_usec - _time_last_ext_vision) > _min_obs_interval_us) {
extVisionSample ev_sample_new;
if ((evdata.time_us - _time_last_ext_vision) > _min_obs_interval_us) {
_time_last_ext_vision = evdata.time_us;
extVisionSample ev_sample_new = evdata;
// calculate the system time-stamp for the mid point of the integration period
ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
ev_sample_new.angVar = evdata->angVar;
ev_sample_new.posVar = evdata->posVar;
ev_sample_new.velVar = evdata->velVar;
ev_sample_new.quat = evdata->quat;
ev_sample_new.pos = evdata->pos;
ev_sample_new.vel = evdata->vel;
_time_last_ext_vision = time_usec;
ev_sample_new.time_us -= _params.ev_delay_ms * 1000;
ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_ext_vision_buffer.push(ev_sample_new);
}
EKF/estimator_interface.h
+1 -1
View File
@@ -189,7 +189,7 @@ public:
void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
// set external vision position and attitude data
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
void setExtVisionData(const extVisionSample& evdata);
void setAuxVelData(const auxVelSample& auxvel_sample);