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

Update setExtVisionData interface

Browse Source
This commit is contained in:
kamilritz 2020-01-20 15:26:07 +01:00 committed by Mathieu Bresciani
parent ed916c8006
commit de33885658
5 changed files with 16 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/common.h
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)

20
EKF/estimator_interface.cpp
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);
}

2
EKF/estimator_interface.h
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);

9
test/sensor_simulator/vio.cpp
View File

@ -15,10 +15,11 @@ Vio::~Vio()
void Vio::send(uint64_t time)
{
_ekf->setExtVisionData(time, &_vio_data);
_vio_data.time_us = time;
_ekf->setExtVisionData(_vio_data);
}
void Vio::setData(const ext_vision_message& vio_data)
void Vio::setData(const extVisionSample& vio_data)
{
_vio_data = vio_data;
}
@ -53,9 +54,9 @@ void Vio::setOrientation(const Quatf& quat)
_vio_data.quat = quat;
}
ext_vision_message Vio::dataAtRest()
extVisionSample Vio::dataAtRest()
{
ext_vision_message vio_data;
extVisionSample vio_data;
vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};

6
test/sensor_simulator/vio.h
View File

@ -50,7 +50,7 @@ public:
Vio(std::shared_ptr<Ekf> ekf);
~Vio();
void setData(const ext_vision_message& vio_data);
void setData(const extVisionSample& vio_data);
void setVelocityVariance(const Vector3f& velVar);
void setPositionVariance(const Vector3f& posVar);
void setAngularVariance(float angVar);
@ -58,10 +58,10 @@ public:
void setPosition(const Vector3f& pos);
void setOrientation(const Quatf& quat);
ext_vision_message dataAtRest();
extVisionSample dataAtRest();
private:
ext_vision_message _vio_data;
extVisionSample _vio_data;
void send(uint64_t time) override;