// update the target angle and total angle traveled
floatangle_change=_angular_vel*dt;
_angle+=angle_change;
_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
Vector3ftarget;
target.x=_center.x+_radius*cosf(-_angle);
target.y=_center.y-_radius*sinf(-_angle);
target.z=_pos_control.get_alt_target();
// update position controller target
_pos_control.set_pos_target(target);
// heading is 180 deg from vehicles target position around circle
_yaw=wrap_PI(_angle-PI);
}else{
// heading is 180 deg from vehicles target position around circle
_yaw=wrap_PI(_angle-PI);
}
// trigger position controller on next update
_pos_control.trigger_xy();
}
// run loiter's position to velocity step
_pos_control.update_pos_controller(false);
}
// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
// this should be called whenever the radius or rate are changed
// initialises the yaw and current position around the circle
voidAC_Circle::calc_velocities()
{
// if we are doing a panorama set the circle_angle to the current heading
if(_radius<=0){
_angle=_ahrs.yaw;
_angular_vel_max=ToRad(_rate);
_angular_accel=_angular_vel_max;// reach maximum yaw velocity in 1 second
}else{
// set starting angle to current heading - 180 degrees
_angle=wrap_PI(_ahrs.yaw-PI);
// calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle