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

Ekf: use helper functions to centralize the checks of horizontal aiding (gps, flow, ev_pos, ev_vel)

Browse Source
This commit is contained in:
bresch
2020-01-24 17:22:50 +04:00
committed by Kabir Mohammed
parent a3e1eb9d50
commit e3af91c9ce
5 changed files with 59 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+25 -1
View File
@@ -374,7 +374,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
delta_time = delta_time_min;
}
const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
const bool relying_on_flow = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
const bool flow_quality_good = (flow.quality >= _params.flow_qual_min);
@@ -571,6 +571,30 @@ bool EstimatorInterface::local_position_is_valid()
return !_deadreckon_time_exceeded;
}
bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
{
return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
}
bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
{
const int nb_sources = getNumberOfActiveHorizontalAidingSources();
return aiding_flag ? nb_sources > 1 : nb_sources > 0;
}
int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
{
return int(_control_status.flags.gps)
+ int(_control_status.flags.opt_flow)
+ int(_control_status.flags.ev_pos)
+ int(_control_status.flags.ev_vel);
}
bool EstimatorInterface::isHorizontalAidingActive() const
{
return getNumberOfActiveHorizontalAidingSources() > 0;
}
void EstimatorInterface::printBufferAllocationFailed(const char * buffer_name)
{
if(buffer_name)