mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 23:14:06 +08:00
532c9abd4a
* Fix comment * Ekf wrapper for testing Add utility function for accessing information in the ekf object * Add step function for Gps sensor * Add RangeFinder and Flow to simulated sensors * Add first fusion logic tests * Add units to function name * Use EXPECT_TRUE * Adding missing qualifiers * Improve EXPECT_ calls * Improve naming
72 lines
1.3 KiB
C++
72 lines
1.3 KiB
C++
#include "ekf_wrapper.h"
|
|
|
|
EkfWrapper::EkfWrapper(std::shared_ptr<Ekf> ekf):
|
|
_ekf{ekf}
|
|
{
|
|
_ekf_params = _ekf->getParamHandle();
|
|
}
|
|
|
|
EkfWrapper::~EkfWrapper()
|
|
{
|
|
}
|
|
|
|
void EkfWrapper::enableGpsFusion()
|
|
{
|
|
_ekf_params->fusion_mode |= MASK_USE_GPS;
|
|
}
|
|
|
|
void EkfWrapper::disableGpsFusion()
|
|
{
|
|
_ekf_params->fusion_mode &= ~MASK_USE_GPS;
|
|
}
|
|
|
|
bool EkfWrapper::isIntendingGpsFusion() const
|
|
{
|
|
filter_control_status_u control_status;
|
|
_ekf->get_control_mode(&control_status.value);
|
|
return control_status.flags.gps;
|
|
}
|
|
|
|
void EkfWrapper::enableFlowFusion()
|
|
{
|
|
_ekf_params->fusion_mode |= MASK_USE_OF;
|
|
}
|
|
|
|
void EkfWrapper::disableFlowFusion()
|
|
{
|
|
_ekf_params->fusion_mode &= ~MASK_USE_OF;
|
|
}
|
|
|
|
bool EkfWrapper::isIntendingFlowFusion() const
|
|
{
|
|
filter_control_status_u control_status;
|
|
_ekf->get_control_mode(&control_status.value);
|
|
return control_status.flags.opt_flow;
|
|
}
|
|
|
|
Vector3f EkfWrapper::getPosition() const
|
|
{
|
|
float temp[3];
|
|
_ekf->get_position(temp);
|
|
return Vector3f(temp);
|
|
}
|
|
Vector3f EkfWrapper::getVelocity() const
|
|
{
|
|
float temp[3];
|
|
_ekf->get_velocity(temp);
|
|
return Vector3f(temp);
|
|
}
|
|
Vector3f EkfWrapper::getAccelBias() const
|
|
{
|
|
float temp[3];
|
|
_ekf->get_accel_bias(temp);
|
|
return Vector3f(temp);
|
|
}
|
|
|
|
Vector3f EkfWrapper::getGyroBias() const
|
|
{
|
|
float temp[3];
|
|
_ekf->get_gyro_bias(temp);
|
|
return Vector3f(temp);
|
|
}
|