2013-03-20 03:27:26 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
# include <AP_HAL.h>
# include <AC_WPNav.h>
extern const AP_HAL : : HAL & hal ;
const AP_Param : : GroupInfo AC_WPNav : : var_info [ ] PROGMEM = {
// index 0 was used for the old orientation matrix
// @Param: SPEED
2013-04-17 23:17:41 -03:00
// @DisplayName: Waypoint Horizontal Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission
2013-06-15 23:39:54 -03:00
// @Units: cm/s
2013-05-07 05:11:24 -03:00
// @Range: 0 2000
2013-04-17 23:17:41 -03:00
// @Increment: 50
2013-04-14 01:24:14 -03:00
// @User: Standard
2013-05-07 05:11:24 -03:00
AP_GROUPINFO ( " SPEED " , 0 , AC_WPNav , _wp_speed_cms , WPNAV_WP_SPEED ) ,
2013-04-08 23:50:12 -03:00
// @Param: RADIUS
// @DisplayName: Waypoint Radius
// @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit.
2013-06-15 23:39:54 -03:00
// @Units: cm
2013-04-08 23:50:12 -03:00
// @Range: 100 1000
// @Increment: 1
// @User: Standard
2013-05-07 05:11:24 -03:00
AP_GROUPINFO ( " RADIUS " , 1 , AC_WPNav , _wp_radius_cm , WPNAV_WP_RADIUS ) ,
2013-03-20 03:27:26 -03:00
2013-04-18 02:51:01 -03:00
// @Param: SPEED_UP
// @DisplayName: Waypoint Climb Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while climbing during a WP mission
2013-06-15 23:39:54 -03:00
// @Units: cm/s
2013-04-17 23:17:41 -03:00
// @Range: 0 1000
// @Increment: 50
// @User: Standard
2013-05-07 05:11:24 -03:00
AP_GROUPINFO ( " SPEED_UP " , 2 , AC_WPNav , _wp_speed_up_cms , WPNAV_WP_SPEED_UP ) ,
2013-04-17 23:17:41 -03:00
2013-04-18 02:51:01 -03:00
// @Param: SPEED_DN
// @DisplayName: Waypoint Descent Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while descending during a WP mission
2013-06-15 23:39:54 -03:00
// @Units: cm/s
2013-04-18 02:51:01 -03:00
// @Range: 0 1000
// @Increment: 50
// @User: Standard
2013-05-07 05:11:24 -03:00
AP_GROUPINFO ( " SPEED_DN " , 3 , AC_WPNav , _wp_speed_down_cms , WPNAV_WP_SPEED_DOWN ) ,
// @Param: LOIT_SPEED
// @DisplayName: Loiter Horizontal Maximum Speed
// @Description: Defines the maximum speed in cm/s which the aircraft will travel horizontally while in loiter mode
2013-06-15 23:39:54 -03:00
// @Units: cm/s
2013-05-07 05:11:24 -03:00
// @Range: 0 2000
// @Increment: 50
// @User: Standard
AP_GROUPINFO ( " LOIT_SPEED " , 4 , AC_WPNav , _loiter_speed_cms , WPNAV_LOITER_SPEED ) ,
2013-06-15 23:39:54 -03:00
// @Param: ACCEL
// @DisplayName: Waypoint Acceleration
// @Description: Defines the horizontal acceleration in cm/s/s used during missions
// @Units: cm/s/s
// @Range: 0 980
// @Increment: 10
// @User: Standard
AP_GROUPINFO ( " ACCEL " , 5 , AC_WPNav , _wp_accel_cms , WPNAV_ACCELERATION ) ,
2013-03-20 03:27:26 -03:00
AP_GROUPEND
} ;
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2014-02-10 10:40:05 -04:00
AC_WPNav : : AC_WPNav ( const AP_InertialNav * inav , const AP_AHRS * ahrs , AC_PosControl & pos_control ) :
2013-03-20 03:27:26 -03:00
_inav ( inav ) ,
2013-05-09 06:32:02 -03:00
_ahrs ( ahrs ) ,
2014-01-17 22:52:33 -04:00
_pos_control ( pos_control ) ,
2013-05-03 04:58:00 -03:00
_loiter_last_update ( 0 ) ,
2014-01-19 06:26:29 -04:00
_loiter_step ( 0 ) ,
_pilot_accel_fwd_cms ( 0 ) ,
_pilot_accel_rgt_cms ( 0 ) ,
2013-06-15 23:39:54 -03:00
_loiter_accel_cms ( WPNAV_LOITER_ACCEL_MAX ) ,
2014-01-19 06:26:29 -04:00
_wp_last_update ( 0 ) ,
_wp_step ( 0 ) ,
_track_length ( 0.0 ) ,
_track_desired ( 0.0 ) ,
_track_accel ( 0.0 ) ,
_track_speed ( 0.0 ) ,
_track_leash_length ( 0.0 )
2013-03-20 03:27:26 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
///
2014-01-19 06:26:29 -04:00
/// loiter controller
2013-03-20 03:27:26 -03:00
///
2014-01-19 06:26:29 -04:00
/// set_loiter_target in cm from home
void AC_WPNav : : set_loiter_target ( const Vector3f & position )
2014-01-17 22:52:33 -04:00
{
2014-01-19 06:26:29 -04:00
// set target position
_pos_control . set_pos_target ( _inav - > get_position ( ) ) ;
// initialise feed forward velocity to zero
_pos_control . set_desired_velocity ( 0 , 0 ) ;
2014-01-23 23:27:26 -04:00
// initialise pos controller speed and acceleration
2014-01-19 06:26:29 -04:00
_pos_control . set_speed_xy ( _loiter_speed_cms ) ;
_pos_control . set_accel_xy ( _loiter_accel_cms ) ;
2014-01-17 22:52:33 -04:00
// initialise pilot input
2014-01-19 06:26:29 -04:00
_pilot_accel_fwd_cms = 0 ;
_pilot_accel_rgt_cms = 0 ;
2014-01-17 22:52:33 -04:00
}
2014-01-19 06:26:29 -04:00
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav : : init_loiter_target ( )
2013-03-27 02:10:02 -03:00
{
2014-01-19 06:26:29 -04:00
Vector3f curr_vel = _inav - > get_velocity ( ) ;
2013-03-27 02:10:02 -03:00
2014-01-19 06:26:29 -04:00
// set target position
_pos_control . set_pos_target ( _inav - > get_position ( ) ) ;
2013-05-14 23:51:26 -03:00
2014-01-19 06:26:29 -04:00
// initialise feed forward velocities to zero
_pos_control . set_desired_velocity ( curr_vel . x , curr_vel . y ) ;
2013-03-27 02:10:02 -03:00
2014-01-19 06:26:29 -04:00
// initialise pos controller speed, acceleration and leash length
// To-Do: will this cause problems for circle which calls this continuously?
_pos_control . set_speed_xy ( _loiter_speed_cms ) ;
_pos_control . set_accel_xy ( _loiter_accel_cms ) ;
2013-03-27 02:10:02 -03:00
2014-01-19 06:26:29 -04:00
// initialise pilot input
_pilot_accel_fwd_cms = 0 ;
_pilot_accel_rgt_cms = 0 ;
2013-04-14 06:27:39 -03:00
}
2014-01-19 06:26:29 -04:00
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void AC_WPNav : : set_pilot_desired_acceleration ( float control_roll , float control_pitch )
2013-04-14 06:27:39 -03:00
{
2014-01-19 06:26:29 -04:00
// convert pilot input to desired acceleration in cm/s/s
_pilot_accel_fwd_cms = - control_pitch * _loiter_accel_cms / 4500.0f ;
_pilot_accel_rgt_cms = control_roll * _loiter_accel_cms / 4500.0f ;
2013-03-27 02:10:02 -03:00
}
2014-01-19 06:26:29 -04:00
/// get_loiter_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav : : get_loiter_stopping_point_xy ( Vector3f & stopping_point ) const
2013-03-20 03:27:26 -03:00
{
2014-01-19 06:26:29 -04:00
_pos_control . get_stopping_point_xy ( stopping_point ) ;
2013-04-01 09:53:40 -03:00
}
2014-01-19 06:26:29 -04:00
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav : : calc_loiter_desired_velocity ( float nav_dt )
2013-04-01 09:53:40 -03:00
{
// range check nav_dt
if ( nav_dt < 0 ) {
return ;
}
2013-05-24 11:45:03 -03:00
// check loiter speed and avoid divide by zero
if ( _loiter_speed_cms < 100.0f ) {
_loiter_speed_cms = 100.0f ;
2013-04-01 09:53:40 -03:00
}
2013-03-20 03:27:26 -03:00
2013-05-24 11:45:03 -03:00
// rotate pilot input to lat/lon frame
2014-01-19 06:26:29 -04:00
Vector2f desired_accel ;
desired_accel . x = ( _pilot_accel_fwd_cms * _ahrs - > cos_yaw ( ) - _pilot_accel_rgt_cms * _ahrs - > sin_yaw ( ) ) ;
desired_accel . y = ( _pilot_accel_fwd_cms * _ahrs - > sin_yaw ( ) + _pilot_accel_rgt_cms * _ahrs - > cos_yaw ( ) ) ;
// get pos_control's feed forward velocity
Vector2f desired_vel = _pos_control . get_desired_velocity ( ) ;
// add pilot commanded acceleration
desired_vel + = desired_accel * nav_dt ;
// reduce velocity with fake wind resistance
if ( desired_vel . x > 0 ) {
desired_vel . x - = ( _loiter_accel_cms - WPNAV_LOITER_ACCEL_MIN ) * nav_dt * desired_vel . x / _loiter_speed_cms ;
desired_vel . x = max ( desired_vel . x - WPNAV_LOITER_ACCEL_MIN * nav_dt , 0 ) ;
} else if ( desired_vel . x < 0 ) {
desired_vel . x - = ( _loiter_accel_cms - WPNAV_LOITER_ACCEL_MIN ) * nav_dt * desired_vel . x / _loiter_speed_cms ;
desired_vel . x = min ( desired_vel . x + WPNAV_LOITER_ACCEL_MIN * nav_dt , 0 ) ;
}
if ( desired_vel . y > 0 ) {
desired_vel . y - = ( _loiter_accel_cms - WPNAV_LOITER_ACCEL_MIN ) * nav_dt * desired_vel . y / _loiter_speed_cms ;
desired_vel . y = max ( desired_vel . y - WPNAV_LOITER_ACCEL_MIN * nav_dt , 0 ) ;
} else if ( desired_vel . y < 0 ) {
desired_vel . y - = ( _loiter_accel_cms - WPNAV_LOITER_ACCEL_MIN ) * nav_dt * desired_vel . y / _loiter_speed_cms ;
desired_vel . y = min ( desired_vel . y + WPNAV_LOITER_ACCEL_MIN * nav_dt , 0 ) ;
}
// constrain and scale the feed forward velocity if necessary
float vel_total = safe_sqrt ( desired_vel . x * desired_vel . x + desired_vel . y * desired_vel . y ) ;
2013-05-14 23:51:26 -03:00
if ( vel_total > _loiter_speed_cms & & vel_total > 0.0f ) {
2014-01-19 06:26:29 -04:00
desired_vel . x = _loiter_speed_cms * desired_vel . x / vel_total ;
desired_vel . y = _loiter_speed_cms * desired_vel . y / vel_total ;
2013-03-20 03:27:26 -03:00
}
2014-01-19 06:26:29 -04:00
// send adjusted feed forward velocity back to position controller
_pos_control . set_desired_velocity ( desired_vel . x , desired_vel . y ) ;
2013-03-20 03:27:26 -03:00
}
2013-03-20 10:28:05 -03:00
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
2014-01-19 06:26:29 -04:00
int32_t AC_WPNav : : get_loiter_bearing_to_target ( ) const
2013-03-20 10:28:05 -03:00
{
2014-01-19 06:26:29 -04:00
return get_bearing_cd ( _inav - > get_position ( ) , _pos_control . get_pos_target ( ) ) ;
2013-03-20 10:28:05 -03:00
}
2014-01-19 06:26:29 -04:00
/// update_loiter - run the loiter controller - should be called at 100hz
2013-03-20 10:28:05 -03:00
void AC_WPNav : : update_loiter ( )
{
2013-08-27 23:33:10 -03:00
// calculate dt
2013-03-20 10:28:05 -03:00
uint32_t now = hal . scheduler - > millis ( ) ;
2013-04-14 06:27:39 -03:00
float dt = ( now - _loiter_last_update ) / 1000.0f ;
2013-03-20 10:28:05 -03:00
2013-08-27 23:33:10 -03:00
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
2014-01-19 06:26:29 -04:00
if ( dt > 0.095f ) {
// double check dt is reasonable
if ( dt > = 1.0f ) {
dt = 0.0 ;
}
2013-08-27 23:33:10 -03:00
// capture time since last iteration
_loiter_last_update = now ;
// translate any adjustments from pilot to loiter target
2014-01-19 06:26:29 -04:00
calc_loiter_desired_velocity ( dt ) ;
// trigger position controller on next update
_pos_control . trigger_xy ( ) ;
} else {
2013-08-27 23:33:10 -03:00
// run loiter's position to velocity step
2014-01-19 06:26:29 -04:00
_pos_control . update_pos_controller ( true ) ;
2013-08-27 23:33:10 -03:00
}
2013-05-07 05:11:24 -03:00
}
2013-03-20 10:28:05 -03:00
2013-03-20 03:27:26 -03:00
///
/// waypoint navigation
///
/// set_destination - set destination using cm from home
2014-01-19 06:26:29 -04:00
void AC_WPNav : : set_wp_destination ( const Vector3f & destination )
2013-03-20 03:27:26 -03:00
{
2014-01-19 06:26:29 -04:00
// if waypoint controller is active and copter has reached the previous waypoint use it for the origin
if ( _flags . reached_destination & & ( ( hal . scheduler - > millis ( ) - _wp_last_update ) < 1000 ) ) {
2013-04-21 07:54:31 -03:00
_origin = _destination ;
2013-04-14 06:27:39 -03:00
} else {
// otherwise calculate origin from the current position and velocity
2014-01-19 06:26:29 -04:00
_pos_control . get_stopping_point_xy ( _origin ) ;
2014-01-25 04:23:27 -04:00
_pos_control . get_stopping_point_z ( _origin ) ;
2013-04-14 06:27:39 -03:00
}
// set origin and destination
2014-01-19 06:26:29 -04:00
set_wp_origin_and_destination ( _origin , destination ) ;
2013-03-20 03:27:26 -03:00
}
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
2014-01-19 06:26:29 -04:00
void AC_WPNav : : set_wp_origin_and_destination ( const Vector3f & origin , const Vector3f & destination )
2013-03-20 03:27:26 -03:00
{
2013-05-03 04:58:00 -03:00
// store origin and destination locations
2013-03-20 03:27:26 -03:00
_origin = origin ;
_destination = destination ;
2013-04-06 10:25:58 -03:00
Vector3f pos_delta = _destination - _origin ;
2013-04-18 02:51:01 -03:00
2013-05-31 09:03:27 -03:00
_track_length = pos_delta . length ( ) ; // get track length
2013-05-14 23:51:26 -03:00
// calculate each axis' percentage of the total distance to the destination
if ( _track_length = = 0.0f ) {
// avoid possible divide by zero
_pos_delta_unit . x = 0 ;
_pos_delta_unit . y = 0 ;
_pos_delta_unit . z = 0 ;
2013-05-24 11:45:03 -03:00
} else {
2013-05-14 23:51:26 -03:00
_pos_delta_unit = pos_delta / _track_length ;
}
2014-01-19 06:26:29 -04:00
2014-01-23 23:27:26 -04:00
// initialise position controller speed and acceleration
_pos_control . set_speed_xy ( _wp_speed_cms ) ;
_pos_control . set_accel_xy ( _wp_accel_cms ) ;
2014-01-25 04:23:27 -04:00
_pos_control . set_speed_z ( - _wp_speed_down_cms , _wp_speed_up_cms ) ;
2014-01-23 23:27:26 -04:00
_pos_control . calc_leash_length_xy ( ) ;
_pos_control . calc_leash_length_z ( ) ;
2014-01-19 06:26:29 -04:00
// calculate leash lengths
2014-01-23 23:27:26 -04:00
calculate_wp_leash_length ( ) ;
2013-04-02 06:23:46 -03:00
2013-05-03 04:58:00 -03:00
// initialise intermediate point to the origin
2014-01-19 06:26:29 -04:00
_pos_control . set_pos_target ( origin ) ;
_track_desired = 0 ; // target is at beginning of track
2013-05-08 12:18:02 -03:00
_flags . reached_destination = false ;
2014-01-19 06:26:29 -04:00
_flags . fast_waypoint = false ; // default waypoint back to slow
2013-05-08 11:20:29 -03:00
2013-05-22 02:17:26 -03:00
// initialise the limited speed to current speed along the track
2013-08-12 10:58:16 -03:00
const Vector3f & curr_vel = _inav - > get_velocity ( ) ;
2013-05-29 23:52:04 -03:00
// get speed along track (note: we convert vertical speed into horizontal speed equivalent)
2013-05-31 09:03:27 -03:00
float speed_along_track = curr_vel . x * _pos_delta_unit . x + curr_vel . y * _pos_delta_unit . y + curr_vel . z * _pos_delta_unit . z ;
2013-05-22 02:17:26 -03:00
_limited_speed_xy_cms = constrain_float ( speed_along_track , 0 , _wp_speed_cms ) ;
2014-01-19 06:26:29 -04:00
}
2013-05-08 12:18:02 -03:00
2014-01-19 06:26:29 -04:00
/// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav : : get_wp_stopping_point_xy ( Vector3f & stopping_point ) const
{
_pos_control . get_stopping_point_xy ( stopping_point ) ;
2013-03-20 03:27:26 -03:00
}
2014-01-19 06:26:29 -04:00
/// advance_wp_target_along_track - move target location along track from origin to destination
void AC_WPNav : : advance_wp_target_along_track ( float dt )
2013-03-20 03:27:26 -03:00
{
float track_covered ;
2013-05-31 09:03:27 -03:00
Vector3f track_error ;
2013-03-20 03:27:26 -03:00
float track_desired_max ;
2013-04-06 11:32:06 -03:00
float track_desired_temp = _track_desired ;
2013-04-06 10:25:58 -03:00
float track_extra_max ;
2013-03-20 03:27:26 -03:00
// get current location
2013-04-08 23:50:12 -03:00
Vector3f curr_pos = _inav - > get_position ( ) ;
Vector3f curr_delta = curr_pos - _origin ;
2013-03-20 03:27:26 -03:00
2013-05-31 09:03:27 -03:00
// calculate how far along the track we are
track_covered = curr_delta . x * _pos_delta_unit . x + curr_delta . y * _pos_delta_unit . y + curr_delta . z * _pos_delta_unit . z ;
Vector3f track_covered_pos = _pos_delta_unit * track_covered ;
track_error = curr_delta - track_covered_pos ;
// calculate the horizontal error
float track_error_xy = safe_sqrt ( track_error . x * track_error . x + track_error . y * track_error . y ) ;
// calculate the vertical error
float track_error_z = fabsf ( track_error . z ) ;
2014-01-23 23:27:26 -04:00
// get position control leash lengths
float leash_xy = _pos_control . get_leash_xy ( ) ;
float leash_z ;
if ( track_error . z > = 0 ) {
leash_z = _pos_control . get_leash_up_z ( ) ;
} else {
leash_z = _pos_control . get_leash_down_z ( ) ;
}
2013-05-31 09:03:27 -03:00
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
2014-01-23 23:27:26 -04:00
track_extra_max = min ( _track_leash_length * ( leash_z - track_error_z ) / leash_z , _track_leash_length * ( leash_xy - track_error_xy ) / leash_xy ) ;
2013-05-31 09:03:27 -03:00
if ( track_extra_max < 0 ) {
track_desired_max = track_covered ;
} else {
track_desired_max = track_covered + track_extra_max ;
}
2013-05-22 02:17:26 -03:00
// get current velocity
2013-08-12 10:58:16 -03:00
const Vector3f & curr_vel = _inav - > get_velocity ( ) ;
2013-05-31 09:03:27 -03:00
// get speed along track
float speed_along_track = curr_vel . x * _pos_delta_unit . x + curr_vel . y * _pos_delta_unit . y + curr_vel . z * _pos_delta_unit . z ;
2013-05-22 02:17:26 -03:00
// calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms ;
2014-02-10 10:40:05 -04:00
float kP = _pos_control . get_pos_xy_kP ( ) ;
2013-05-22 02:17:26 -03:00
if ( kP > = 0.0f ) { // avoid divide by zero
2013-05-31 09:03:27 -03:00
linear_velocity = _track_accel / kP ;
2013-04-29 22:20:23 -03:00
}
2013-05-22 02:17:26 -03:00
// let the limited_speed_xy_cms be some range above or below current velocity along track
if ( speed_along_track < - linear_velocity ) {
// we are travelling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
_limited_speed_xy_cms = 0 ;
} else {
// increase intermediate target point's velocity if not yet at target speed (we will limit it below)
2013-05-31 09:03:27 -03:00
if ( dt > 0 ) {
if ( track_desired_max > _track_desired ) {
2013-12-10 19:21:41 -04:00
_limited_speed_xy_cms + = 2.0f * _track_accel * dt ;
2013-05-31 09:03:27 -03:00
} else {
// do nothing, velocity stays constant
_track_desired = track_desired_max ;
}
2013-05-22 02:17:26 -03:00
}
// do not go over top speed
2013-05-31 09:03:27 -03:00
if ( _limited_speed_xy_cms > _track_speed ) {
_limited_speed_xy_cms = _track_speed ;
2013-05-22 02:17:26 -03:00
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if ( fabsf ( speed_along_track ) < linear_velocity ) {
_limited_speed_xy_cms = constrain_float ( _limited_speed_xy_cms , speed_along_track - linear_velocity , speed_along_track + linear_velocity ) ;
}
2013-04-29 22:20:23 -03:00
}
2013-03-20 03:27:26 -03:00
// advance the current target
2013-05-31 09:03:27 -03:00
track_desired_temp + = _limited_speed_xy_cms * dt ;
2013-04-06 11:32:06 -03:00
2013-04-05 06:55:23 -03:00
// do not let desired point go past the end of the segment
2013-05-01 21:25:40 -03:00
track_desired_temp = constrain_float ( track_desired_temp , 0 , _track_length ) ;
2013-04-06 11:32:06 -03:00
_track_desired = max ( _track_desired , track_desired_temp ) ;
2013-03-20 03:27:26 -03:00
// recalculate the desired position
2014-01-19 06:26:29 -04:00
_pos_control . set_pos_target ( _origin + _pos_delta_unit * _track_desired ) ;
2013-04-08 23:50:12 -03:00
// check if we've reached the waypoint
2013-05-08 12:18:02 -03:00
if ( ! _flags . reached_destination ) {
2013-04-08 23:50:12 -03:00
if ( _track_desired > = _track_length ) {
2013-05-08 12:18:02 -03:00
// "fast" waypoints are complete once the intermediate point reaches the destination
if ( _flags . fast_waypoint ) {
_flags . reached_destination = true ;
} else {
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = curr_pos - _destination ;
if ( dist_to_dest . length ( ) < = _wp_radius_cm ) {
_flags . reached_destination = true ;
}
2013-04-08 23:50:12 -03:00
}
}
}
2013-03-20 03:27:26 -03:00
}
2014-01-19 06:26:29 -04:00
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav : : get_wp_distance_to_destination ( )
2013-03-20 03:27:26 -03:00
{
2013-03-20 10:28:05 -03:00
// get current location
Vector3f curr = _inav - > get_position ( ) ;
return pythagorous2 ( _destination . x - curr . x , _destination . y - curr . y ) ;
}
2013-03-20 03:27:26 -03:00
2014-01-19 06:26:29 -04:00
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav : : get_wp_bearing_to_destination ( )
2013-03-20 10:28:05 -03:00
{
return get_bearing_cd ( _inav - > get_position ( ) , _destination ) ;
}
2013-03-20 03:27:26 -03:00
2013-03-20 10:28:05 -03:00
/// update_wpnav - run the wp controller - should be called at 10hz
void AC_WPNav : : update_wpnav ( )
{
2013-08-27 23:33:10 -03:00
// calculate dt
2013-03-20 10:28:05 -03:00
uint32_t now = hal . scheduler - > millis ( ) ;
2014-01-19 06:26:29 -04:00
float dt = ( now - _wp_last_update ) / 1000.0f ;
2013-08-27 23:33:10 -03:00
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
2014-01-19 06:26:29 -04:00
if ( dt > 0.095f ) {
// double check dt is reasonable
if ( dt > = 1.0f ) {
dt = 0.0 ;
}
2013-08-27 23:33:10 -03:00
// capture time since last iteration
2014-01-19 06:26:29 -04:00
_wp_last_update = now ;
2013-08-27 23:33:10 -03:00
// advance the target if necessary
2014-01-19 06:26:29 -04:00
advance_wp_target_along_track ( dt ) ;
_pos_control . trigger_xy ( ) ;
} else {
// run position controller
_pos_control . update_pos_controller ( false ) ;
2013-08-27 23:33:10 -03:00
}
2013-03-20 03:27:26 -03:00
}
2013-05-07 05:11:24 -03:00
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
2014-01-23 23:27:26 -04:00
void AC_WPNav : : calculate_wp_leash_length ( )
2013-04-18 02:51:01 -03:00
{
2013-05-31 09:03:27 -03:00
// length of the unit direction vector in the horizontal
float pos_delta_unit_xy = sqrt ( _pos_delta_unit . x * _pos_delta_unit . x + _pos_delta_unit . y * _pos_delta_unit . y ) ;
float pos_delta_unit_z = fabsf ( _pos_delta_unit . z ) ;
2014-01-23 23:27:26 -04:00
float speed_z ;
float leash_z ;
if ( _pos_delta_unit . z > = 0 ) {
speed_z = _wp_speed_up_cms ;
leash_z = _pos_control . get_leash_up_z ( ) ;
} else {
speed_z = _wp_speed_down_cms ;
leash_z = _pos_control . get_leash_down_z ( ) ;
}
2013-05-31 09:03:27 -03:00
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if ( pos_delta_unit_z = = 0 & & pos_delta_unit_xy = = 0 ) {
_track_accel = 0 ;
_track_speed = 0 ;
_track_leash_length = WPNAV_MIN_LEASH_LENGTH ;
} else if ( _pos_delta_unit . z = = 0 ) {
2013-06-15 23:39:54 -03:00
_track_accel = _wp_accel_cms / pos_delta_unit_xy ;
2013-05-31 09:03:27 -03:00
_track_speed = _wp_speed_cms / pos_delta_unit_xy ;
2014-01-23 23:27:26 -04:00
_track_leash_length = _pos_control . get_leash_xy ( ) / pos_delta_unit_xy ;
2013-05-31 09:03:27 -03:00
} else if ( pos_delta_unit_xy = = 0 ) {
_track_accel = WPNAV_ALT_HOLD_ACCEL_MAX / pos_delta_unit_z ;
2014-01-23 23:27:26 -04:00
_track_speed = speed_z / pos_delta_unit_z ;
_track_leash_length = leash_z / pos_delta_unit_z ;
} else {
2013-06-15 23:39:54 -03:00
_track_accel = min ( WPNAV_ALT_HOLD_ACCEL_MAX / pos_delta_unit_z , _wp_accel_cms / pos_delta_unit_xy ) ;
2014-01-23 23:27:26 -04:00
_track_speed = min ( speed_z / pos_delta_unit_z , _wp_speed_cms / pos_delta_unit_xy ) ;
_track_leash_length = min ( leash_z / pos_delta_unit_z , _pos_control . get_leash_xy ( ) / pos_delta_unit_xy ) ;
}
}
///
/// shared methods
///
// get_bearing_cd - return bearing in centi-degrees between two positions
// To-Do: move this to math library
float AC_WPNav : : get_bearing_cd ( const Vector3f & origin , const Vector3f & destination ) const
{
float bearing = 9000 + atan2f ( - ( destination . x - origin . x ) , destination . y - origin . y ) * 5729.57795f ;
if ( bearing < 0 ) {
bearing + = 36000 ;
2013-05-29 23:52:04 -03:00
}
2014-01-23 23:27:26 -04:00
return bearing ;
2013-04-21 07:54:31 -03:00
}