Copter: Provide downward facing rangefinder readings to proximity lib

This commit is contained in:
Rishabh 2021-02-20 00:26:00 +05:30 committed by Randy Mackay
parent e9c0e50185
commit e727d08266
1 changed files with 4 additions and 1 deletions

View File

@ -90,12 +90,15 @@ void Copter::read_rangefinder(void)
rf_state.last_healthy_ms = now; rf_state.last_healthy_ms = now;
} }
// send downward facing lidar altitude and health to waypoint and circle navigation libraries // send downward facing lidar altitude and health to the libraries that require it
if (rf_orient == ROTATION_PITCH_270) { if (rf_orient == ROTATION_PITCH_270) {
if (rangefinder_state.alt_healthy || timed_out) { if (rangefinder_state.alt_healthy || timed_out) {
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
#if MODE_CIRCLE_ENABLED #if MODE_CIRCLE_ENABLED
circle_nav->set_rangefinder_alt(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); circle_nav->set_rangefinder_alt(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
#endif
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
#endif #endif
} }
} }