2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AC_Circle.h"
# include <AP_Math/AP_Math.h>
2014-01-27 01:08:11 -04:00
extern const AP_HAL : : HAL & hal ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AC_Circle : : var_info [ ] = {
2014-01-27 01:08:11 -04:00
// @Param: RADIUS
// @DisplayName: Circle Radius
// @Description: Defines the radius of the circle the vehicle will fly when in Circle flight mode
// @Units: cm
// @Range: 0 10000
// @Increment: 100
// @User: Standard
AP_GROUPINFO ( " RADIUS " , 0 , AC_Circle , _radius , AC_CIRCLE_RADIUS_DEFAULT ) ,
// @Param: RATE
// @DisplayName: Circle rate
// @Description: Circle mode's turn rate in deg/sec. Positive to turn clockwise, negative for counter clockwise
// @Units: deg/s
// @Range: -90 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " RATE " , 1 , AC_Circle , _rate , AC_CIRCLE_RATE_DEFAULT ) ,
AP_GROUPEND
} ;
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2017-02-11 18:33:43 -04:00
AC_Circle : : AC_Circle ( const AP_InertialNav & inav , const AP_AHRS_View & ahrs , AC_PosControl & pos_control ) :
2014-01-27 01:08:11 -04:00
_inav ( inav ) ,
_ahrs ( ahrs ) ,
_pos_control ( pos_control ) ,
2014-07-14 11:25:41 -03:00
_yaw ( 0.0f ) ,
_angle ( 0.0f ) ,
_angle_total ( 0.0f ) ,
_angular_vel ( 0.0f ) ,
_angular_vel_max ( 0.0f ) ,
_angular_accel ( 0.0f )
2014-01-27 01:08:11 -04:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2015-06-08 01:29:46 -03:00
// init flags
_flags . panorama = false ;
2014-01-27 01:08:11 -04:00
}
2014-04-16 04:19:41 -03:00
/// init - initialise circle controller setting center specifically
2014-05-07 03:05:03 -03:00
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
2014-04-16 04:19:41 -03:00
void AC_Circle : : init ( const Vector3f & center )
2014-01-27 01:08:11 -04:00
{
2014-04-16 04:19:41 -03:00
_center = center ;
2014-01-27 01:08:11 -04:00
2014-05-07 03:05:03 -03:00
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
2017-07-13 08:45:45 -03:00
_pos_control . set_desired_accel_xy ( 0.0f , 0.0f ) ;
_pos_control . set_desired_velocity_xy ( 0.0f , 0.0f ) ;
2014-05-07 03:05:03 -03:00
_pos_control . init_xy_controller ( ) ;
2014-01-27 01:08:11 -04:00
2014-05-07 03:05:03 -03:00
// set initial position target to reasonable stopping point
_pos_control . set_target_to_stopping_point_xy ( ) ;
_pos_control . set_target_to_stopping_point_z ( ) ;
2014-01-27 01:08:11 -04:00
// calculate velocities
2015-07-22 09:40:20 -03:00
calc_velocities ( true ) ;
2014-04-16 04:19:41 -03:00
// set start angle from position
init_start_angle ( false ) ;
2014-01-27 01:08:11 -04:00
}
2014-04-16 04:19:41 -03:00
/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
2014-05-07 03:05:03 -03:00
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
2014-04-16 04:19:41 -03:00
void AC_Circle : : init ( )
2014-01-27 01:08:11 -04:00
{
2014-05-07 03:05:03 -03:00
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
2017-07-13 08:45:45 -03:00
_pos_control . set_desired_accel_xy ( 0.0f , 0.0f ) ;
_pos_control . set_desired_velocity_xy ( 0.0f , 0.0f ) ;
2014-05-07 03:05:03 -03:00
_pos_control . init_xy_controller ( ) ;
2014-01-27 01:08:11 -04:00
2014-05-07 03:05:03 -03:00
// 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 ( ) ;
2014-01-27 01:08:11 -04:00
// set circle center to circle_radius ahead of stopping point
2014-02-08 23:34:50 -04:00
_center . x = stopping_point . x + _radius * _ahrs . cos_yaw ( ) ;
_center . y = stopping_point . y + _radius * _ahrs . sin_yaw ( ) ;
2014-01-27 01:08:11 -04:00
_center . z = stopping_point . z ;
// calculate velocities
2015-07-22 09:40:20 -03:00
calc_velocities ( true ) ;
2014-04-16 04:19:41 -03:00
// set starting angle from vehicle heading
init_start_angle ( true ) ;
2014-01-27 01:08:11 -04:00
}
2015-07-22 09:40:20 -03:00
/// set_circle_rate - set circle rate in degrees per second
void AC_Circle : : set_rate ( float deg_per_sec )
{
2016-04-16 06:59:20 -03:00
if ( ! is_equal ( deg_per_sec , _rate . get ( ) ) ) {
2015-07-22 09:40:20 -03:00
_rate = deg_per_sec ;
calc_velocities ( false ) ;
}
}
2014-01-27 01:08:11 -04:00
/// update - update circle controller
void AC_Circle : : update ( )
{
// calculate dt
2014-12-17 17:26:13 -04:00
float dt = _pos_control . time_since_last_xy_update ( ) ;
2018-03-08 22:40:56 -04:00
if ( dt > = 0.2f ) {
dt = 0.0f ;
}
2014-01-27 01:08:11 -04:00
2018-03-08 22:40:56 -04:00
// ramp angular velocity to maximum
if ( _angular_vel < _angular_vel_max ) {
_angular_vel + = fabsf ( _angular_accel ) * dt ;
_angular_vel = MIN ( _angular_vel , _angular_vel_max ) ;
}
if ( _angular_vel > _angular_vel_max ) {
_angular_vel - = fabsf ( _angular_accel ) * dt ;
_angular_vel = MAX ( _angular_vel , _angular_vel_max ) ;
2014-01-27 01:08:11 -04:00
}
2018-03-08 22:40:56 -04:00
// update the target angle and total angle traveled
float angle_change = _angular_vel * dt ;
_angle + = angle_change ;
_angle = wrap_PI ( _angle ) ;
_angle_total + = angle_change ;
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if ( ! is_zero ( _radius ) ) {
// calculate target position
Vector3f target ;
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_xy_target ( target . x , target . y ) ;
// heading is 180 deg from vehicles target position around circle
_yaw = wrap_PI ( _angle - M_PI ) * DEGX100 ;
} else {
// 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_xy_target ( target . x , target . y ) ;
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * DEGX100 ;
}
// update position controller
_pos_control . update_xy_controller ( 1.0f ) ;
2014-01-27 01:08:11 -04:00
}
2014-04-16 04:19:41 -03:00
// get_closest_point_on_circle - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void AC_Circle : : get_closest_point_on_circle ( Vector3f & result )
{
// return center if radius is zero
if ( _radius < = 0 ) {
result = _center ;
return ;
}
// get current position
2018-07-20 03:08:41 -03:00
Vector3f stopping_point ;
_pos_control . get_stopping_point_xy ( stopping_point ) ;
2014-04-16 04:19:41 -03:00
2018-07-20 03:08:41 -03:00
// calc vector from stopping point to circle center
2014-04-16 04:19:41 -03:00
Vector2f vec ; // vector from circle center to current location
2018-07-20 03:08:41 -03:00
vec . x = ( stopping_point . x - _center . x ) ;
vec . y = ( stopping_point . y - _center . y ) ;
2016-04-16 06:58:46 -03:00
float dist = norm ( vec . x , vec . y ) ;
2014-04-16 04:19:41 -03:00
// if current location is exactly at the center of the circle return edge directly behind vehicle
2015-05-04 23:34:54 -03:00
if ( is_zero ( dist ) ) {
2014-04-16 04:19:41 -03:00
result . x = _center . x - _radius * _ahrs . cos_yaw ( ) ;
result . y = _center . y - _radius * _ahrs . sin_yaw ( ) ;
result . z = _center . z ;
return ;
}
// calculate closest point on edge of circle
result . x = _center . x + vec . x / dist * _radius ;
result . y = _center . y + vec . y / dist * _radius ;
result . z = _center . z ;
}
2014-01-27 01:08:11 -04:00
// 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
2015-07-22 09:40:20 -03:00
void AC_Circle : : calc_velocities ( bool init_velocity )
2014-01-27 01:08:11 -04:00
{
// if we are doing a panorama set the circle_angle to the current heading
if ( _radius < = 0 ) {
_angular_vel_max = ToRad ( _rate ) ;
2015-11-27 13:11:58 -04:00
_angular_accel = MAX ( fabsf ( _angular_vel_max ) , ToRad ( AC_CIRCLE_ANGULAR_ACCEL_MIN ) ) ; // reach maximum yaw velocity in 1 second
2014-01-27 01:08:11 -04:00
} else {
// 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
2015-11-27 13:11:58 -04:00
float velocity_max = MIN ( _pos_control . get_speed_xy ( ) , safe_sqrt ( 0.5f * _pos_control . get_accel_xy ( ) * _radius ) ) ;
2014-01-27 01:08:11 -04:00
// angular_velocity in radians per second
_angular_vel_max = velocity_max / _radius ;
_angular_vel_max = constrain_float ( ToRad ( _rate ) , - _angular_vel_max , _angular_vel_max ) ;
// angular_velocity in radians per second
2015-11-27 13:11:58 -04:00
_angular_accel = MAX ( _pos_control . get_accel_xy ( ) / _radius , ToRad ( AC_CIRCLE_ANGULAR_ACCEL_MIN ) ) ;
2014-01-27 01:08:11 -04:00
}
2014-04-16 04:19:41 -03:00
// initialise angular velocity
2015-07-22 09:40:20 -03:00
if ( init_velocity ) {
_angular_vel = 0 ;
}
2014-01-27 01:08:11 -04:00
}
2014-04-16 04:19:41 -03:00
// init_start_angle - sets the starting angle around the circle and initialises the angle_total
// if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
// if use_heading is false the vehicle's position from the center will be used to initialise the angle
void AC_Circle : : init_start_angle ( bool use_heading )
{
// initialise angle total
_angle_total = 0 ;
// if the radius is zero we are doing panorama so init angle to the current heading
if ( _radius < = 0 ) {
_angle = _ahrs . yaw ;
return ;
}
// if use_heading is true
if ( use_heading ) {
2016-02-25 13:13:02 -04:00
_angle = wrap_PI ( _ahrs . yaw - M_PI ) ;
2014-04-16 04:19:41 -03:00
} else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f & curr_pos = _inav . get_position ( ) ;
2015-05-04 23:34:54 -03:00
if ( is_equal ( curr_pos . x , _center . x ) & & is_equal ( curr_pos . y , _center . y ) ) {
2016-02-25 13:13:02 -04:00
_angle = wrap_PI ( _ahrs . yaw - M_PI ) ;
2014-04-16 04:19:41 -03:00
} else {
// get bearing from circle center to vehicle in radians
2015-05-04 19:42:05 -03:00
float bearing_rad = atan2f ( curr_pos . y - _center . y , curr_pos . x - _center . x ) ;
2014-04-16 04:19:41 -03:00
_angle = wrap_PI ( bearing_rad ) ;
}
}
}