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

terrain_estimator: add unit tests

Browse Source
This commit is contained in:
bresch
2020-06-03 11:24:26 +02:00
committed by Mathieu Bresciani
parent aa96fa6d9e
commit 821e1fa8fc
6 changed files with 223 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/ekf_wrapper.cpp
+34
View File
@@ -177,6 +177,40 @@ bool EkfWrapper::isWindVelocityEstimated() const
return control_status.flags.wind;
}
void EkfWrapper::enableTerrainRngFusion()
{
_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseRangeFinder;
}
void EkfWrapper::disableTerrainRngFusion()
{
_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseRangeFinder;
}
bool EkfWrapper::isIntendingTerrainRngFusion() const
{
terrain_fusion_status_u terrain_status;
terrain_status.value = _ekf->getTerrainEstimateSensorBitfield();
return terrain_status.flags.range_finder;
}
void EkfWrapper::enableTerrainFlowFusion()
{
_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseOpticalFlow;
}
void EkfWrapper::disableTerrainFlowFusion()
{
_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseOpticalFlow;
}
bool EkfWrapper::isIntendingTerrainFlowFusion() const
{
terrain_fusion_status_u terrain_status;
terrain_status.value = _ekf->getTerrainEstimateSensorBitfield();
return terrain_status.flags.flow;
}
Eulerf EkfWrapper::getEulerAngles() const
{
return Eulerf(_ekf->getQuaternion());