Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Circle: z-axis target only updated during terrain following

Browse Source 
This allows the circle flight mode to externally control the altitude target
This commit is contained in:
Randy Mackay 2020-04-20 09:33:36 +09:00
parent 2af70f5e60
commit 3195a7cccd
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -181,13 +181,21 @@ bool AC_Circle::update()
return false; return false;
} }
// calculate z-axis target
float target_z_cm;
if (_terrain_alt) {
target_z_cm = _center.z + terr_offset;
} else {
target_z_cm = _pos_control.get_alt_target();
}
// if the circle_radius is zero we are doing panorama so no need to update loiter target // if the circle_radius is zero we are doing panorama so no need to update loiter target
if (!is_zero(_radius)) { if (!is_zero(_radius)) {
// calculate target position // calculate target position
Vector3f target; Vector3f target;
target.x = _center.x + _radius * cosf(-_angle); target.x = _center.x + _radius * cosf(-_angle);
target.y = _center.y - _radius * sinf(-_angle); target.y = _center.y - _radius * sinf(-_angle);
target.z = _center.z + terr_offset; target.z = target_z_cm;
// update position controller target // update position controller target
_pos_control.set_pos_target(target); _pos_control.set_pos_target(target);
@ -199,7 +207,7 @@ bool AC_Circle::update()
Vector3f target; Vector3f target;
target.x = _center.x; target.x = _center.x;
target.y = _center.y; target.y = _center.y;
target.z = _center.z + terr_offset; target.z = target_z_cm;
// update position controller target // update position controller target
_pos_control.set_pos_target(target); _pos_control.set_pos_target(target);