Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Add support for terrain altitude time constant

This commit is contained in:
Leonard Hall 2023-03-04 11:31:45 +10:30 committed by Peter Barker
parent 7978347044
commit 6930e8872d
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduSub/sensors.cpp
View File

@ -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;