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

Test external vision

Browse Source
This commit is contained in:
kamilritz
2019-12-18 11:42:12 +01:00
committed by Mathieu Bresciani
parent 84dcb32bd8
commit 1b0e137b8a
10 changed files with 388 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/ekf_wrapper.cpp
+68
View File
@@ -44,6 +44,67 @@ bool EkfWrapper::isIntendingFlowFusion() const
return control_status.flags.opt_flow;
}
void EkfWrapper::enableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode |= MASK_USE_EVPOS;
}
void EkfWrapper::disableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode &= ~MASK_USE_EVPOS;
}
bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_pos;
}
void EkfWrapper::enableExternalVisionVelocityFusion()
{
_ekf_params->fusion_mode |= MASK_USE_EVVEL;
}
void EkfWrapper::disableExternalVisionVelocityFusion()
{
_ekf_params->fusion_mode &= ~MASK_USE_EVVEL;
}
bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_vel;
}
void EkfWrapper::enableExternalVisionHeadingFusion()
{
_ekf_params->fusion_mode |= MASK_USE_EVYAW;
}
void EkfWrapper::disableExternalVisionHeadingFusion()
{
_ekf_params->fusion_mode &= ~MASK_USE_EVYAW;
}
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_yaw;
}
void EkfWrapper::enableExternalVisionAlignment()
{
_ekf_params->fusion_mode |= MASK_ROTATE_EV;
}
void EkfWrapper::disableExternalVisionAlignment()
{
_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
}
Vector3f EkfWrapper::getPosition() const
{
float temp[3];
@@ -101,3 +162,10 @@ Vector3f EkfWrapper::getVelocityVariance() const
{
return Vector3f(_ekf->velocity_covariances().diag());
}
Quatf EkfWrapper::getVisionAlignmentQuaternion() const
{
float temp[4];
_ekf->get_ev2ekf_quaternion(temp);
return Quatf(temp);
}