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: bug fix for pos target when radius is zero

Browse Source
This commit is contained in:
Randy Mackay 2014-02-16 11:44:30 +09:00 committed by Andrew Tridgell
parent 9130c88f15
commit 6af705d455
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -111,9 +111,6 @@ void AC_Circle::update()
_angle = wrap_PI(_angle);
_angle_total += angle_change;
// heading is 180 deg from vehicles target position around circle
_yaw = wrap_PI(_angle-PI);
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if (_radius != 0.0) {
// calculate target position
@ -126,10 +123,19 @@ void AC_Circle::update()
_pos_control.set_pos_target(target);
// heading is 180 deg from vehicles target position around circle
_yaw = wrap_PI(_angle-PI);
_yaw = wrap_PI(_angle-PI) * AC_CIRCLE_DEGX100;
}else{
// heading is 180 deg from vehicles target position around circle
_yaw = wrap_PI(_angle-PI);
// set target position to center
Vector3f target;
target.x = _center.x;
target.y = _center.y;
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_pos_target(target);
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * AC_CIRCLE_DEGX100;
}
// trigger position controller on next update

2
libraries/AC_WPNav/AC_Circle.h
View File

@ -12,6 +12,8 @@
#define AC_CIRCLE_RADIUS_DEFAULT 1000.0f // radius of the circle in cm that the vehicle will fly
#define AC_CIRCLE_RATE_DEFAULT 20.0f // turn rate in deg/sec. Positive to turn clockwise, negative for counter clockwise
#define AC_CIRCLE_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
class AC_Circle
{
public: