2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
# include <AP_Math/AP_Math.h>
2020-04-08 03:20:56 -03:00
# include <AP_Terrain/AP_Terrain.h>
# include "AC_Circle.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
2020-01-06 18:35:17 -04:00
// @Range: 0 200000
2014-01-27 01:08:11 -04:00
// @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 ) ,
2020-01-06 18:35:17 -04:00
// @Param: CONTROL
// @DisplayName: Circle control
// @Description: Enable or disable using the pitch/roll stick control circle mode's radius and rate
// @Values: 0:Disable,1:Enable
// @User: Standard
AP_GROUPINFO ( " CONTROL " , 2 , AC_Circle , _control , 1 ) ,
2014-01-27 01:08:11 -04:00
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
2020-04-08 03:20:56 -03:00
/// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain
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
2020-04-08 03:20:56 -03:00
void AC_Circle : : init ( const Vector3f & center , bool terrain_alt )
2014-01-27 01:08:11 -04:00
{
2014-04-16 04:19:41 -03:00
_center = center ;
2020-04-08 03:20:56 -03:00
_terrain_alt = terrain_alt ;
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 ;
2020-04-08 03:20:56 -03:00
_terrain_alt = false ;
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 starting angle from vehicle heading
init_start_angle ( true ) ;
2014-01-27 01:08:11 -04:00
}
2020-04-08 03:20:56 -03:00
/// set circle center to a Location
void AC_Circle : : set_center ( const Location & center )
{
if ( center . get_alt_frame ( ) = = Location : : AltFrame : : ABOVE_TERRAIN ) {
// convert Location with terrain altitude
Vector2f center_xy ;
int32_t terr_alt_cm ;
if ( center . get_vector_xy_from_origin_NE ( center_xy ) & & center . get_alt_cm ( Location : : AltFrame : : ABOVE_TERRAIN , terr_alt_cm ) ) {
set_center ( Vector3f ( center_xy . x , center_xy . y , terr_alt_cm ) , true ) ;
} else {
// failed to convert location so set to current position and log error
set_center ( _inav . get_position ( ) , false ) ;
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : NAVIGATION , LogErrorCode : : FAILED_CIRCLE_INIT ) ;
}
} else {
// convert Location with alt-above-home, alt-above-origin or absolute alt
Vector3f circle_center_neu ;
if ( ! center . get_vector_from_origin_NEU ( circle_center_neu ) ) {
// default to current position and log error
circle_center_neu = _inav . get_position ( ) ;
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : NAVIGATION , LogErrorCode : : FAILED_CIRCLE_INIT ) ;
}
set_center ( circle_center_neu , false ) ;
}
}
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 ;
}
}
2020-01-06 18:35:17 -04:00
/// set_circle_rate - set circle rate in degrees per second
void AC_Circle : : set_radius ( float radius_cm )
{
_radius = constrain_float ( radius_cm , 0 , AC_CIRCLE_RADIUS_MAX ) ;
}
2020-04-21 08:42:32 -03:00
/// returns true if update has been run recently
/// used by vehicle code to determine if get_yaw() is valid
bool AC_Circle : : is_active ( ) const
{
return ( AP_HAL : : millis ( ) - _last_update_ms < 200 ) ;
}
2014-01-27 01:08:11 -04:00
/// update - update circle controller
2020-04-08 03:20:56 -03:00
bool AC_Circle : : update ( )
2014-01-27 01:08:11 -04:00
{
2020-06-23 08:32:34 -03:00
calc_velocities ( false ) ;
2014-01-27 01:08:11 -04:00
// 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 ;
2020-04-08 03:20:56 -03:00
// calculate terrain adjustments
float terr_offset = 0.0f ;
if ( _terrain_alt & & ! get_terrain_offset ( terr_offset ) ) {
return false ;
}
2020-04-19 21:33:36 -03:00
// calculate z-axis target
float target_z_cm ;
if ( _terrain_alt ) {
target_z_cm = _center . z + terr_offset ;
} else {
target_z_cm = _pos_control . get_alt_target ( ) ;
}
2018-03-08 22:40:56 -04:00
// 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 ) ;
2020-04-19 21:33:36 -03:00
target . z = target_z_cm ;
2018-03-08 22:40:56 -04:00
// update position controller target
2020-04-08 03:20:56 -03:00
_pos_control . set_pos_target ( target ) ;
2018-03-08 22:40:56 -04:00
2019-04-24 23:35:19 -03:00
// heading is from vehicle to center of circle
_yaw = get_bearing_cd ( _inav . get_position ( ) , _center ) ;
2018-03-08 22:40:56 -04:00
} else {
// set target position to center
Vector3f target ;
target . x = _center . x ;
target . y = _center . y ;
2020-04-19 21:33:36 -03:00
target . z = target_z_cm ;
2018-03-08 22:40:56 -04:00
// update position controller target
2020-04-08 03:20:56 -03:00
_pos_control . set_pos_target ( target ) ;
2018-03-08 22:40:56 -04:00
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * DEGX100 ;
}
// update position controller
2018-09-05 06:44:08 -03:00
_pos_control . update_xy_controller ( ) ;
2020-04-08 03:20:56 -03:00
2020-04-21 08:42:32 -03:00
// set update time
_last_update_ms = AP_HAL : : millis ( ) ;
2020-04-08 03:20:56 -03:00
return true ;
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
2020-01-06 18:35:17 -04:00
void AC_Circle : : get_closest_point_on_circle ( Vector3f & result ) const
2014-04-16 04:19:41 -03:00
{
// 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
2018-09-20 16:40:41 -03:00
float velocity_max = MIN ( _pos_control . get_max_speed_xy ( ) , safe_sqrt ( 0.5f * _pos_control . get_max_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
2018-09-20 16:40:41 -03:00
_angular_accel = MAX ( _pos_control . get_max_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 ) ;
}
}
}
2020-04-08 03:20:56 -03:00
// get expected source of terrain data
AC_Circle : : TerrainSource AC_Circle : : get_terrain_source ( ) const
{
// use range finder if connected
2020-04-09 02:06:24 -03:00
if ( _rangefinder_available ) {
2020-04-08 03:20:56 -03:00
return AC_Circle : : TerrainSource : : TERRAIN_FROM_RANGEFINDER ;
}
# if AP_TERRAIN_AVAILABLE
const AP_Terrain * terrain = AP_Terrain : : get_singleton ( ) ;
if ( ( terrain ! = nullptr ) & & terrain - > enabled ( ) ) {
return AC_Circle : : TerrainSource : : TERRAIN_FROM_TERRAINDATABASE ;
} else {
return AC_Circle : : TerrainSource : : TERRAIN_UNAVAILABLE ;
}
# else
return AC_Circle : : TerrainSource : : TERRAIN_UNAVAILABLE ;
# endif
}
// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool AC_Circle : : get_terrain_offset ( float & offset_cm )
{
// calculate offset based on source (rangefinder or terrain database)
switch ( get_terrain_source ( ) ) {
case AC_Circle : : TerrainSource : : TERRAIN_UNAVAILABLE :
return false ;
case AC_Circle : : TerrainSource : : TERRAIN_FROM_RANGEFINDER :
if ( _rangefinder_healthy ) {
offset_cm = _inav . get_altitude ( ) - _rangefinder_alt_cm ;
return true ;
}
return false ;
case AC_Circle : : TerrainSource : : TERRAIN_FROM_TERRAINDATABASE :
# if AP_TERRAIN_AVAILABLE
float terr_alt = 0.0f ;
AP_Terrain * terrain = AP_Terrain : : get_singleton ( ) ;
if ( terrain ! = nullptr & & terrain - > height_above_terrain ( terr_alt , true ) ) {
offset_cm = _inav . get_altitude ( ) - ( terr_alt * 100.0f ) ;
return true ;
}
# endif
return false ;
}
// we should never get here but just in case
return false ;
}