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

Copter: reduce Circle init twitch by using stopping point

Browse Source
This commit is contained in:
Randy Mackay 2014-05-07 15:05:03 +09:00
parent 3cc5b9446f
commit 1cb297580f
2 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -41,14 +41,17 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosCont
}
/// init - initialise circle controller setting center specifically
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
void AC_Circle::init(const Vector3f& center)
{
_center = center;
// To-Do: set target position, angle, etc so that copter begins circle from closest point to stopping point
_pos_control.set_pos_target(_inav.get_position());
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
_pos_control.init_xy_controller();
// To-Do: set _pos_control speed and accel
// set initial position target to reasonable stopping point
_pos_control.set_target_to_stopping_point_xy();
_pos_control.set_target_to_stopping_point_z();
// calculate velocities
calc_velocities();
@ -58,22 +61,24 @@ void AC_Circle::init(const Vector3f& center)
}
/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
void AC_Circle::init()
{
Vector3f stopping_point;
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
_pos_control.init_xy_controller();
// get reasonable stopping point
_pos_control.get_stopping_point_xy(stopping_point);
_pos_control.get_stopping_point_z(stopping_point);
// set initial position target to reasonable stopping point
_pos_control.set_target_to_stopping_point_xy();
_pos_control.set_target_to_stopping_point_z();
// get stopping point
const Vector3f& stopping_point = _pos_control.get_pos_target();
// set circle center to circle_radius ahead of stopping point
_center.x = stopping_point.x + _radius * _ahrs.cos_yaw();
_center.y = stopping_point.y + _radius * _ahrs.sin_yaw();
_center.z = stopping_point.z;
// update pos_control target to stopping point
_pos_control.set_pos_target(stopping_point);
// calculate velocities
calc_velocities();

2
libraries/AC_WPNav/AC_Circle.h
View File

@ -22,9 +22,11 @@ public:
AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control);
/// init - initialise circle controller setting center specifically
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
void init(const Vector3f& center);
/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
void init();
/// set_circle_center in cm from home