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

Update range finder interface

Browse Source
This commit is contained in:
kamilritz
2020-01-21 14:50:21 +01:00
committed by Paul Riseborough
parent 4fb2d1b02e
commit 7c3a2f839b
2 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+5 -8
View File
@@ -452,7 +452,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
}
}
void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &velocity, const Vector3f &variance)
void EstimatorInterface::setAuxVelData(const auxVelSample& auxvel_sample)
{
if (!_initialised || _auxvel_buffer_fail) {
return;
@@ -470,16 +470,13 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &veloc
}
// limit data rate to prevent data being lost
if ((time_usec - _time_last_auxvel) > _min_obs_interval_us) {
if ((auxvel_sample.time_us - _time_last_auxvel) > _min_obs_interval_us) {
_time_last_auxvel = auxvel_sample.time_us;
auxVelSample auxvel_sample_new;
auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000;
auxVelSample auxvel_sample_new = auxvel_sample;
auxvel_sample_new.time_us -= _params.auxvel_delay_ms * 1000;
auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_auxvel = time_usec;
auxvel_sample_new.vel = velocity;
auxvel_sample_new.velVar = variance;
_auxvel_buffer.push(auxvel_sample_new);
}
EKF/estimator_interface.h
+1 -1
View File
@@ -193,7 +193,7 @@ public:
// set external vision position and attitude data
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance);
void setAuxVelData(const auxVelSample& auxvel_sample);
// return a address to the parameters struct
// in order to give access to the application