Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: use prev wp as origin for next wp

Browse Source
This commit is contained in:
Randy Mackay 2013-04-14 18:27:39 +09:00
parent 629d23b7e2
commit 0eab3e87b2
2 changed files with 41 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -48,18 +48,18 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
/// simple loiter controller
///
/// set_loiter_target - set initial loiter target based on current position and velocity
void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity)
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
Vector3f AC_WPNav::project_stopping_point(const Vector3f& position, const Vector3f& velocity)
{
float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
float linear_velocity; // the velocity we swap between linear and sqrt.
float vel_total;
float target_dist;
Vector3f target;
// avoid divide by zero
if( _pid_pos_lat->kP() <= 0.1 ) {
set_loiter_target(position);
return;
return(position);
}
// calculate point at which velocity switches from linear to sqrt
@ -77,8 +77,18 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
}
target_dist = constrain(target_dist, 0, MAX_LOITER_OVERSHOOT);
_target.x = position.x + (target_dist * velocity.x / vel_total);
_target.y = position.y + (target_dist * velocity.y / vel_total);
target.x = position.x + (target_dist * velocity.x / vel_total);
target.y = position.y + (target_dist * velocity.y / vel_total);
target.z = position.z;
return target;
}
/// set_loiter_target - set initial loiter target based on current position and velocity
void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity)
{
Vector3f target = project_stopping_point(position, velocity);
_target.x = target.x;
_target.y = target.y;
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
@ -155,15 +165,13 @@ int32_t AC_WPNav::get_bearing_to_target()
void AC_WPNav::update_loiter()
{
uint32_t now = hal.scheduler->millis();
float dt = (now - _last_update) / 1000.0f;
_last_update = now;
float dt = (now - _loiter_last_update) / 1000.0f;
_loiter_last_update = now;
// catch if we've just been started
if( dt >= 1.0 ) {
dt = 0.0;
reset_I();
_target_vel.x = 0;
_target_vel.y = 0;
}
// translate any adjustments from pilot to loiter target
@ -180,8 +188,18 @@ void AC_WPNav::update_loiter()
/// set_destination - set destination using cm from home
void AC_WPNav::set_destination(const Vector3f& destination)
{
// To-Do: use projection of current position & velocity to set origin
set_origin_and_destination(_inav->get_position(), destination);
Vector3f origin;
// if waypoint controlls is active and copter has reached the previous waypoint use it for the origin
if( _reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
origin = _destination;
}else{
// otherwise calculate origin from the current position and velocity
origin = project_stopping_point(_inav->get_position(), _inav->get_velocity());
}
// set origin and destination
set_origin_and_destination(origin, destination);
}
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
@ -272,8 +290,8 @@ int32_t AC_WPNav::get_bearing_to_destination()
void AC_WPNav::update_wpnav()
{
uint32_t now = hal.scheduler->millis();
float dt = (now - _last_update) / 1000.0f;
_last_update = now;
float dt = (now - _wpnav_last_update) / 1000.0f;
_wpnav_last_update = now;
// catch if we've just been started
if( dt >= 1.0 ) {
@ -414,4 +432,8 @@ void AC_WPNav::reset_I()
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
// reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0;
_target_vel.y = 0;
}

6
libraries/AC_WPNav/AC_WPNav.h
View File

@ -149,6 +149,9 @@ public:
protected:
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
Vector3f project_stopping_point(const Vector3f& position, const Vector3f& velocity);
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void translate_loiter_target_movements(float nav_dt);
@ -183,7 +186,8 @@ protected:
AP_Float _speed_cms; // default horizontal speed in cm/s
float _speedz_cms; // max vertical climb rate in cm/s. To-Do: rename or pull this from main code
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
uint32_t _last_update; // time of last update call
uint32_t _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw;
float _cos_roll;