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

ekf2: refactor vertical acceleration check for multiple height sources

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent 8fd79688c0
commit f9188b2a14
7 changed files with 230 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
+30
View File
@@ -528,6 +528,36 @@ bool EstimatorInterface::isHorizontalAidingActive() const
return getNumberOfActiveHorizontalAidingSources() > 0;
}
bool EstimatorInterface::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const
{
const int nb_sources = getNumberOfActiveVerticalPositionAidingSources();
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
}
bool EstimatorInterface::isVerticalPositionAidingActive() const
{
return getNumberOfActiveVerticalPositionAidingSources() > 0;
}
int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
{
return int(_control_status.flags.gps_hgt)
+ int(_control_status.flags.baro_hgt)
+ int(_control_status.flags.rng_hgt)
+ int(_control_status.flags.ev_hgt);
}
bool EstimatorInterface::isVerticalVelocityAidingActive() const
{
return getNumberOfActiveVerticalVelocityAidingSources() > 0;
}
int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
{
return int(_control_status.flags.gps)
+ int(_control_status.flags.ev_vel);
}
void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
{
if (buffer_name) {