Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: add support for visual odometry

Browse Source
This commit is contained in:
Randy Mackay 2020-04-06 13:14:10 +09:00
parent d1aa3858f0
commit ac3f99d8dd
1 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduSub/system.cpp
View File

@ -247,11 +247,24 @@ bool Sub::ekf_position_ok()
// optflow_position_ok - returns true if optical flow based position estimate is ok // optflow_position_ok - returns true if optical flow based position estimate is ok
bool Sub::optflow_position_ok() bool Sub::optflow_position_ok()
{ {
#if OPTFLOW != ENABLED // return immediately if EKF not used
if (!ahrs.have_inertial_nav()) {
return false; return false;
#else }
// return immediately if optflow is not enabled or EKF not used
if (!optflow.enabled() || !ahrs.have_inertial_nav()) { // return immediately if neither optflow nor visual odometry is enabled
bool enabled = false;
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
enabled = true;
}
#endif
#if HAL_VISUALODOM_ENABLED
if (visual_odom.enabled()) {
enabled = true;
}
#endif
if (!enabled) {
return false; return false;
} }
@ -263,7 +276,6 @@ bool Sub::optflow_position_ok()
return (filt_status.flags.pred_horiz_pos_rel); return (filt_status.flags.pred_horiz_pos_rel);
} }
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
#endif
} }
/* /*