aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Guided: support terrain following

Browse Source
This commit is contained in:
Leonard Hall 2021-07-08 18:16:52 +09:30 committed by Randy Mackay
parent e0e283f13e
commit 8631c3c68f
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/mode_guided.cpp
View File

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