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

EKF: RingBuffer minor improvements

Browse Source 
- allocate IMU and output buffers on construction according to defaults
 - determine buffer max time delay based on configuration parameters
 - reorder flowSample and extVisionSample to minimize padding
 - adjust parameter defaults to match PX4-Autopilot
This commit is contained in:
Daniel Agar
2020-12-03 20:48:51 -05:00
committed by GitHub
parent 67f13f7ede
commit 03cfcb903e
12 changed files with 85 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/baro.cpp
+1 -2
View File
@@ -15,8 +15,7 @@ Baro::~Baro()
void Baro::send(uint64_t time)
{
const baroSample baro_sample {_baro_data, time};
_ekf->setBaroData(baro_sample);
_ekf->setBaroData(baroSample{time, _baro_data});
}
void Baro::setData(float baro)
test/sensor_simulator/ekf_wrapper.cpp
+4 -12
View File
@@ -17,9 +17,7 @@ void EkfWrapper::setBaroHeight()
bool EkfWrapper::isIntendingBaroHeightFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.baro_hgt;
return _ekf->control_status_flags().baro_hgt;
}
void EkfWrapper::setGpsHeight()
@@ -29,9 +27,7 @@ void EkfWrapper::setGpsHeight()
bool EkfWrapper::isIntendingGpsHeightFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.gps_hgt;
return _ekf->control_status_flags().gps_hgt;
}
void EkfWrapper::setRangeHeight()
@@ -41,9 +37,7 @@ void EkfWrapper::setRangeHeight()
bool EkfWrapper::isIntendingRangeHeightFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.rng_hgt;
return _ekf->control_status_flags().rng_hgt;
}
void EkfWrapper::setVisionHeight()
@@ -53,9 +47,7 @@ void EkfWrapper::setVisionHeight()
bool EkfWrapper::isIntendingVisionHeightFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_hgt;
return _ekf->control_status_flags().ev_hgt;
}
void EkfWrapper::enableGpsFusion()
test/sensor_simulator/mag.cpp
+1 -2
View File
@@ -15,8 +15,7 @@ Mag::~Mag()
void Mag::send(uint64_t time)
{
const magSample mag_sample {_mag_data, time};
_ekf->setMagData(mag_sample);
_ekf->setMagData(magSample{time, _mag_data});
}
void Mag::setData(const Vector3f& mag)
test/sensor_simulator/vio.cpp
+3 -4
View File
@@ -61,15 +61,14 @@ void Vio::setOrientation(const Quatf& quat)
void Vio::setVelocityFrameToBody()
{
_vio_data.vel_frame = BODY_FRAME_FRD;
_vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
}
void Vio::setVelocityFrameToLocal()
{
_vio_data.vel_frame = LOCAL_FRAME_FRD;
_vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
}
extVisionSample Vio::dataAtRest()
{
extVisionSample vio_data;
@@ -79,7 +78,7 @@ extVisionSample Vio::dataAtRest()
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velCov = matrix::eye<float ,3>() * 0.1f;
vio_data.angVar = 0.05f;
vio_data.vel_frame = LOCAL_FRAME_FRD;
vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
return vio_data;
}