From e727d0826691f7b6874f6bceda48d4e441e60885 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sat, 20 Feb 2021 00:26:00 +0530 Subject: [PATCH] Copter: Provide downward facing rangefinder readings to proximity lib --- ArduCopter/sensors.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index aeafed1177..4c0efd147b 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -90,12 +90,15 @@ void Copter::read_rangefinder(void) 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 (rangefinder_state.alt_healthy || timed_out) { wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); #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()); +#endif +#if PROXIMITY_ENABLED == ENABLED + g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); #endif } }