/// 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(); }