From 6930e8872dfb0f96a49d0217886a77c4eea01668 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 4 Mar 2023 11:31:45 +1030 Subject: [PATCH] Sub: Add support for terrain altitude time constant --- ArduSub/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 8a0ff2a2c9..481930cd75 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -55,8 +55,9 @@ void Sub::read_rangefinder() } // send rangefinder altitude and health to waypoint navigation library - wp_nav.set_rangefinder_alt(rangefinder_state.enabled, 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()); + const float terrain_offset_cm = inertial_nav.get_position_z_up_cm() - rangefinder_state.alt_cm_filt.get(); + wp_nav.set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, terrain_offset_cm); + circle_nav.set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav.rangefinder_used(), rangefinder_state.alt_healthy, terrain_offset_cm); #else rangefinder_state.enabled = false;