/// set_circle_rate - set circle rate in degrees per second
void AC_Circle::set_rate(float deg_per_sec)
{
if (!is_equal(deg_per_sec,_rate)) {
_rate = deg_per_sec;
calc_velocities(false);
}
}
/// 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()
{
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
_pos_control.set_desired_accel_xy(0.0f,0.0f);
_pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.init_xy_controller();
// 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;
// calculate velocities
calc_velocities(true);
// set starting angle from vehicle heading
init_start_angle(true);
}
/// set_circle_center in cm from home
void AC_Circle::set_center(const Vector3f& position)
{
_center = position;
// 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());
// To-Do: set _pos_control speed and accel
// calculate velocities
calc_velocities();
}
/// 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;
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
_pos_control.init_xy_controller();
// 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(true);
// set start angle from position
init_start_angle(false);
}
/// init_center in cm from home using stopping point and projecting out based on the copter's heading
void AC_Circle::init_center()
{
Vector3f stopping_point;
// get reasonable stopping point
_pos_control.get_stopping_point_xy(stopping_point);
_pos_control.get_stopping_point_z(stopping_point);
// 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();
}
