From 6ff5913aa1a2b5104dabbc7e76ad028203084808 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 18:16:52 +0930 Subject: [PATCH] Copter: Guided: support terrain following --- ArduCopter/mode_guided.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 1be591832b..e256289bc4 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -563,7 +563,14 @@ void ModeGuided::pos_control_run() // send position and velocity targets to position controller guided_accel_target_cmss.zero(); guided_vel_target_cms.zero(); - pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset); + + // todo: Randy to convert to parameter TERRAIN_MARGIN (in m) + float TERRAIN_MARGIN = 10; + float pos_offset_z_buffer = 0.0; // Vertical buffer size in m + if (guided_pos_terrain_alt) { + pos_offset_z_buffer = MIN(TERRAIN_MARGIN * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); + } + pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer); // run position controllers pos_control->update_xy_controller();