Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: remove xy pos controller

Browse Source
This commit is contained in:
Randy Mackay 2014-01-19 19:26:29 +09:00 committed by Andrew Tridgell
parent d380e6b4d3
commit 0c8cbba644
2 changed files with 203 additions and 481 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

534
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -68,7 +68,8 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control,
APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
_inav(inav),
_ahrs(ahrs),
_pos_control(pos_control),
@ -77,26 +78,20 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon),
_loiter_last_update(0),
_wpnav_last_update(0),
_althold_kP(WPNAV_ALT_HOLD_P),
_desired_roll(0),
_desired_pitch(0),
_target(0,0,0),
_pilot_vel_forward_cms(0),
_pilot_vel_right_cms(0),
_target_vel(0,0,0),
_vel_last(0,0,0),
_loiter_step(0),
_pilot_accel_fwd_cms(0),
_pilot_accel_rgt_cms(0),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_lean_angle_max_cd(MAX_LEAN_ANGLE),
_wp_last_update(0),
_wp_step(0),
_track_length(0.0),
_track_desired(0.0),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
_track_accel(0),
_track_speed(0),
_track_leash_length(0),
dist_error(0,0),
desired_vel(0,0),
desired_accel(0,0)
_track_accel(0.0),
_track_speed(0.0),
_track_leash_length(0.0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -106,97 +101,74 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
}
///
/// simple loiter controller
/// loiter controller
///
/// init_loiter_target - sets initial loiter target based on current position and velocity
void AC_WPNav::init_loiter_target()
{
_pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity());
// initialise pilot input
_pilot_vel_forward_cms = 0;
_pilot_vel_right_cms = 0;
}
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const
{
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;
float kP = _pid_pos_lat->kP();
// calculate current velocity
vel_total = safe_sqrt(velocity.x*velocity.x + velocity.y*velocity.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (vel_total < 10.0f || kP <= 0.0f || _wp_accel_cms <= 0.0f) {
target = position;
return;
}
// calculate point at which velocity switches from linear to sqrt
linear_velocity = _wp_accel_cms/kP;
// calculate distance within which we can stop
if (vel_total < linear_velocity) {
target_dist = vel_total/kP;
} else {
linear_distance = _wp_accel_cms/(2.0f*kP*kP);
target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms);
}
target_dist = constrain_float(target_dist, 0, _wp_leash_xy);
target.x = position.x + (target_dist * velocity.x / vel_total);
target.y = position.y + (target_dist * velocity.y / vel_total);
target.z = position.z;
}
/// set_loiter_target in cm from home
void AC_WPNav::set_loiter_target(const Vector3f& position)
{
_target = position;
_target_vel.x = 0;
_target_vel.y = 0;
}
// set target position
_pos_control.set_pos_target(_inav->get_position());
/// init_loiter_target - set initial loiter target based on current position and velocity
void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velocity)
{
// set target position and velocity based on current pos and velocity
_target.x = position.x;
_target.y = position.y;
_target_vel.x = velocity.x;
_target_vel.y = velocity.y;
// initialise feed forward velocity to zero
_pos_control.set_desired_velocity(0,0);
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the loiter controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
// initialise pos controller speed, acceleration and leash length
_pos_control.set_speed_xy(_loiter_speed_cms);
_pos_control.set_accel_xy(_loiter_accel_cms);
_pos_control.set_leash_xy(_loiter_leash);
// initialise pilot input
_pilot_vel_forward_cms = 0;
_pilot_vel_right_cms = 0;
// set last velocity to current velocity
// To-Do: remove the line below by instead forcing reset_I to be called on the first loiter_update call
_vel_last = _inav->get_velocity();
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float dt)
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::init_loiter_target()
{
// convert pilot input to desired velocity in cm/s
_pilot_vel_forward_cms = -control_pitch * _loiter_accel_cms / 4500.0f;
_pilot_vel_right_cms = control_roll * _loiter_accel_cms / 4500.0f;
Vector3f curr_vel = _inav->get_velocity();
// set target position
_pos_control.set_pos_target(_inav->get_position());
// initialise feed forward velocities to zero
_pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
// 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);
_pos_control.set_leash_xy(_loiter_leash);
// initialise pilot input
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;
}
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void AC_WPNav::translate_loiter_target_movements(float nav_dt)
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void AC_WPNav::calculate_loiter_leash_length()
{
Vector2f target_vel_adj;
float vel_total;
_loiter_leash = _pos_control.calc_leash_length_xy(_loiter_speed_cms,_loiter_accel_cms);
}
/// 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)
{
// 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;
}
/// 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
{
_pos_control.get_stopping_point_xy(stopping_point);
}
/// 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)
{
// range check nav_dt
if( nav_dt < 0 ) {
return;
@ -208,167 +180,102 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
}
// rotate pilot input to lat/lon frame
target_vel_adj.x = (_pilot_vel_forward_cms*_ahrs->cos_yaw() - _pilot_vel_right_cms*_ahrs->sin_yaw());
target_vel_adj.y = (_pilot_vel_forward_cms*_ahrs->sin_yaw() + _pilot_vel_right_cms*_ahrs->cos_yaw());
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());
// add desired change in velocity to current target velocit
_target_vel.x += target_vel_adj.x*nav_dt;
_target_vel.y += target_vel_adj.y*nav_dt;
if(_target_vel.x > 0 ) {
_target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
_target_vel.x = max(_target_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(_target_vel.x < 0) {
_target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
_target_vel.x = min(_target_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
// 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(_target_vel.y > 0 ) {
_target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
_target_vel.y = max(_target_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(_target_vel.y < 0) {
_target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
_target_vel.y = min(_target_vel.y + 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 the velocity vector and scale if necessary
vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y);
// 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);
if (vel_total > _loiter_speed_cms && vel_total > 0.0f) {
_target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total;
_target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total;
desired_vel.x = _loiter_speed_cms * desired_vel.x/vel_total;
desired_vel.y = _loiter_speed_cms * desired_vel.y/vel_total;
}
// update target position
_target.x += _target_vel.x * nav_dt;
_target.y += _target_vel.y * nav_dt;
// constrain target position to within reasonable distance of current location
Vector3f curr_pos = _inav->get_position();
Vector3f distance_err = _target - curr_pos;
float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y);
if (distance > _loiter_leash && distance > 0.0f) {
_target.x = curr_pos.x + _loiter_leash * distance_err.x/distance;
_target.y = curr_pos.y + _loiter_leash * distance_err.y/distance;
}
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity(desired_vel.x,desired_vel.y);
}
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_bearing_to_target() const
int32_t AC_WPNav::get_loiter_bearing_to_target() const
{
return get_bearing_cd(_inav->get_position(), _pos_control.get_post_target());
return get_bearing_cd(_inav->get_position(), _pos_control.get_pos_target());
}
/// update_loiter - run the loiter controller - should be called at 10hz
/// update_loiter - run the loiter controller - should be called at 100hz
void AC_WPNav::update_loiter()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _loiter_last_update) / 1000.0f;
// catch if we've just been started
if( dt >= 1.0f ) {
dt = 0.0f;
reset_I();
_loiter_step = 0;
}
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f && _loiter_step > 3) {
_loiter_step = 0;
}
// run loiter steps
switch (_loiter_step) {
case 0:
if (dt > 0.095f) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
}
// capture time since last iteration
_loiter_dt = dt;
_loiter_last_update = now;
// translate any adjustments from pilot to loiter target
translate_loiter_target_movements(_loiter_dt);
_loiter_step++;
break;
case 1:
// run loiter's position to velocity step
get_loiter_position_to_velocity(_loiter_dt, WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR);
_loiter_step++;
break;
case 2:
// run loiter's velocity to acceleration step
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _loiter_dt);
_loiter_step++;
break;
case 3:
// run loiter's acceleration to lean angle step
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
_loiter_step++;
break;
}
}
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void AC_WPNav::calculate_loiter_leash_length()
{
// get loiter position P
float kP = _pid_pos_lat->kP();
// check loiter speed
if( _loiter_speed_cms < 100.0f) {
_loiter_speed_cms = 100.0f;
}
// set loiter acceleration to 1/2 loiter speed
_loiter_accel_cms = _loiter_speed_cms / 2.0f;
// avoid divide by zero
if (kP <= 0.0f || _wp_accel_cms <= 0.0f) {
_loiter_leash = WPNAV_MIN_LEASH_LENGTH;
return;
}
// calculate horizontal leash length
if(WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR <= _wp_accel_cms / kP) {
// linear leash length based on speed close in
_loiter_leash = WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / kP;
calc_loiter_desired_velocity(dt);
// trigger position controller on next update
_pos_control.trigger_xy();
}else{
// leash length grows at sqrt of speed further out
_loiter_leash = (_wp_accel_cms / (2.0f*kP*kP)) + (WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR*WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / (2.0f*_wp_accel_cms));
}
// ensure leash is at least 1m long
if( _loiter_leash < WPNAV_MIN_LEASH_LENGTH ) {
_loiter_leash = WPNAV_MIN_LEASH_LENGTH;
// run loiter's position to velocity step
_pos_control.update_pos_controller(true);
}
}
///
/// waypoint navigation
///
/// set_destination - set destination using cm from home
void AC_WPNav::set_destination(const Vector3f& destination)
void AC_WPNav::set_wp_destination(const Vector3f& destination)
{
// if waypoint controlls is active and copter has reached the previous waypoint use it for the origin
if( _flags.reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
// 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) ) {
_origin = _destination;
}else{
// otherwise calculate origin from the current position and velocity
get_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin);
_pos_control.get_stopping_point_xy(_origin);
}
// set origin and destination
set_origin_and_destination(_origin, destination);
set_wp_origin_and_destination(_origin, destination);
}
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
Vector3f pos_delta = _destination - _origin;
// calculate leash lengths
bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
_track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination
@ -380,33 +287,32 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
}else{
_pos_delta_unit = pos_delta/_track_length;
}
calculate_wp_leash_length(climb); // update leash lengths
// calculate leash lengths
bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
calculate_wp_leash_length(climb);
// initialise intermediate point to the origin
_track_desired = 0;
_target = origin;
_pos_control.set_pos_target(origin);
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
// initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav->get_velocity();
// get speed along track (note: we convert vertical speed into horizontal speed equivalent)
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;
_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
// default waypoint back to slow
_flags.fast_waypoint = false;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the wpnav controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
// reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0;
_target_vel.y = 0;
}
/// advance_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_target_along_track(float dt)
/// 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);
}
/// advance_wp_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_wp_target_along_track(float dt)
{
float track_covered;
Vector3f track_error;
@ -481,7 +387,7 @@ void AC_WPNav::advance_target_along_track(float dt)
_track_desired = max(_track_desired, track_desired_temp);
// recalculate the desired position
_target = _origin + _pos_delta_unit * _track_desired;
_pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired);
// check if we've reached the waypoint
if( !_flags.reached_destination ) {
@ -500,16 +406,16 @@ void AC_WPNav::advance_target_along_track(float dt)
}
}
/// get_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_distance_to_destination()
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_wp_distance_to_destination()
{
// get current location
Vector3f curr = _inav->get_position();
return pythagorous2(_destination.x-curr.x,_destination.y-curr.y);
}
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_bearing_to_destination()
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_wp_bearing_to_destination()
{
return get_bearing_cd(_inav->get_position(), _destination);
}
@ -519,48 +425,23 @@ void AC_WPNav::update_wpnav()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _wpnav_last_update) / 1000.0f;
// catch if we've just been started
if( dt >= 1.0f ) {
dt = 0.0;
reset_I();
_wpnav_step = 0;
}
float dt = (now - _wp_last_update) / 1000.0f;
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f && _wpnav_step > 3) {
_wpnav_step = 0;
}
// run loiter steps
switch (_wpnav_step) {
case 0:
if (dt > 0.095f) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
}
// capture time since last iteration
_wpnav_dt = dt;
_wpnav_last_update = now;
_wp_last_update = now;
// advance the target if necessary
if (dt > 0.0f) {
advance_target_along_track(dt);
}
_wpnav_step++;
break;
case 1:
// run loiter's position to velocity step
get_loiter_position_to_velocity(_wpnav_dt, _wp_speed_cms);
_wpnav_step++;
break;
case 2:
// run loiter's velocity to acceleration step
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _wpnav_dt);
_wpnav_step++;
break;
case 3:
// run loiter's acceleration to lean angle step
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
_wpnav_step++;
break;
advance_wp_target_along_track(dt);
_pos_control.trigger_xy();
}else{
// run position controller
_pos_control.update_pos_controller(false);
}
}
@ -568,112 +449,6 @@ void AC_WPNav::update_wpnav()
/// shared methods
///
/// get_loiter_position_to_velocity - loiter position controller
/// converts desired position held in _target vector to desired velocity
void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
{
Vector3f curr = _inav->get_position();
float dist_error_total;
float vel_sqrt;
float vel_total;
float linear_distance; // the distace we swap between linear and sqrt.
float kP = _pid_pos_lat->kP();
// avoid divide by zero
if (kP <= 0.0f) {
desired_vel.x = 0.0;
desired_vel.y = 0.0;
}else{
// calculate distance error
dist_error.x = _target.x - curr.x;
dist_error.y = _target.y - curr.y;
linear_distance = _wp_accel_cms/(2.0f*kP*kP);
dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y);
_distance_to_target = dist_error_total; // for reporting purposes
if( dist_error_total > 2.0f*linear_distance ) {
vel_sqrt = safe_sqrt(2.0f*_wp_accel_cms*(dist_error_total-linear_distance));
desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
}else{
desired_vel.x = _pid_pos_lat->kP() * dist_error.x;
desired_vel.y = _pid_pos_lon->kP() * dist_error.y;
}
// ensure velocity stays within limits
vel_total = safe_sqrt(desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y);
if( vel_total > max_speed_cms ) {
desired_vel.x = max_speed_cms * desired_vel.x/vel_total;
desired_vel.y = max_speed_cms * desired_vel.y/vel_total;
}
// feed forward velocity request
desired_vel.x += _target_vel.x;
desired_vel.y += _target_vel.y;
}
}
/// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_WPNav::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt)
{
const Vector3f &vel_curr = _inav->get_velocity(); // current velocity in cm/s
Vector3f vel_error; // The velocity error in cm/s.
float accel_total; // total acceleration in cm/s/s
// reset last velocity if this controller has just been engaged or dt is zero
if( dt == 0.0f ) {
desired_accel.x = 0;
desired_accel.y = 0;
} else {
// feed forward desired acceleration calculation
desired_accel.x = (vel_lat - _vel_last.x)/dt;
desired_accel.y = (vel_lon - _vel_last.y)/dt;
}
// store this iteration's velocities for the next iteration
_vel_last.x = vel_lat;
_vel_last.y = vel_lon;
// calculate velocity error
vel_error.x = vel_lat - vel_curr.x;
vel_error.y = vel_lon - vel_curr.y;
// combine feed foward accel with PID outpu from velocity error
desired_accel.x += _pid_rate_lat->get_pid(vel_error.x, dt);
desired_accel.y += _pid_rate_lon->get_pid(vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
accel_total = safe_sqrt(desired_accel.x*desired_accel.x + desired_accel.y*desired_accel.y);
if( accel_total > WPNAV_ACCEL_MAX ) {
desired_accel.x = WPNAV_ACCEL_MAX * desired_accel.x/accel_total;
desired_accel.y = WPNAV_ACCEL_MAX * desired_accel.y/accel_total;
}
}
/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float accel_lon)
{
float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s
float accel_forward;
float accel_right;
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = accel_lat*_ahrs->cos_yaw() + accel_lon*_ahrs->sin_yaw();
accel_right = -accel_lat*_ahrs->sin_yaw() + accel_lon*_ahrs->cos_yaw();
// update angle targets that will be passed to stabilize controller
_desired_roll = constrain_float(fast_atan(accel_right*_ahrs->cos_pitch()/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
_desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
}
// 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
@ -685,24 +460,13 @@ float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destinati
return bearing;
}
/// reset_I - clears I terms from loiter PID controller
void AC_WPNav::reset_I()
{
_pid_pos_lon->reset_I();
_pid_pos_lat->reset_I();
_pid_rate_lon->reset_I();
_pid_rate_lat->reset_I();
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
}
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_wp_leash_length(bool climb)
{
// get loiter position P
float kP = _pid_pos_lat->kP();
float althold_kP = _pos_control.althold_kP();
// sanity check acceleration and avoid divide by zero
if (_wp_accel_cms <= 0.0f) {
@ -714,7 +478,7 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
return;
}
// calculate horiztonal leash length
// calculate horizontal leash length
if(_wp_speed_cms <= _wp_accel_cms / kP) {
// linear leash length based on speed close in
_wp_leash_xy = _wp_speed_cms / kP;
@ -735,12 +499,12 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
}else{
speed_vert = _wp_speed_down_cms;
}
if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / _althold_kP) {
if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / althold_kP) {
// linear leash length based on speed close in
_wp_leash_z = speed_vert / _althold_kP;
_wp_leash_z = speed_vert / althold_kP;
}else{
// leash length grows at sqrt of speed further out
_wp_leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*_althold_kP*_althold_kP)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX));
_wp_leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*althold_kP*althold_kP)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX));
}
// ensure leash is at least 1m long

150
libraries/AC_WPNav/AC_WPNav.h
View File

@ -2,7 +2,6 @@
#ifndef AC_WPNAV_H
#define AC_WPNAV_H
#include <inttypes.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
@ -44,60 +43,64 @@ public:
AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
///
/// simple loiter controller
/// loiter controller
///
/// get_loiter_target - get loiter target as position vector (from home in cm)
const Vector3f &get_loiter_target() const { return _target; }
/// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position);
/// init_loiter_target - sets initial loiter target based on current position and velocity
void init_loiter_target() { _pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity()); }
/// move_loiter_target - move destination using pilot input
void move_loiter_target(float control_roll, float control_pitch, float dt);
/// get_distance_to_target - get horizontal distance to loiter target in cm
float get_distance_to_target() const;
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_bearing_to_target() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const;
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void init_loiter_target();
/// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
void set_loiter_velocity(float velocity_cms) { _loiter_speed_cms = velocity_cms; };
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void set_pilot_desired_acceleration(float control_roll, float control_pitch);
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_loiter_stopping_point_xy(Vector3f& stopping_point) const;
/// get_loiter_distance_to_target - get horizontal distance to loiter target in cm
float get_loiter_distance_to_target() const { return _pos_control.get_distance_to_target(); }
/// get_loiter_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_loiter_bearing_to_target() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
///
/// waypoint controller
///
/// get_destination waypoint using position vector (distance from home in cm)
const Vector3f &get_destination() const { return _destination; }
/// get_wp_destination waypoint using position vector (distance from home in cm)
const Vector3f &get_wp_destination() const { return _destination; }
/// set_destination waypoint using position vector (distance from home in cm)
void set_destination(const Vector3f& destination);
/// set_wp_destination waypoint using position vector (distance from home in cm)
void set_wp_destination(const Vector3f& destination);
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
void set_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
/// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
void set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
/// advance_target_along_track - move target location along track from origin to destination
void advance_target_along_track(float dt);
/// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
/// results placed in stopping_position vector
void get_wp_stopping_point_xy(Vector3f& stopping_point) const;
/// get_distance_to_destination - get horizontal distance to destination in cm
float get_distance_to_destination();
/// advance_wp_target_along_track - move target location along track from origin to destination
void advance_wp_target_along_track(float dt);
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float get_wp_distance_to_destination();
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t get_bearing_to_destination();
int32_t get_wp_bearing_to_destination();
/// reached_destination - true when we have come within RADIUS cm of the waypoint
bool reached_destination() const { return _flags.reached_destination; }
bool reached_wp_destination() const { return _flags.reached_destination; }
/// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it
void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; }
@ -110,17 +113,14 @@ public:
///
/// get desired roll, pitch which should be fed into stabilize controllers
int32_t get_desired_roll() const { return _desired_roll; };
int32_t get_desired_pitch() const { return _desired_pitch; };
int32_t get_roll() const { return _pos_control.get_roll(); };
int32_t get_pitch() const { return _pos_control.get_pitch(); };
/// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
float get_desired_alt() const { return _target.z; }
float get_desired_alt() const { return _pos_control.get_alt_target(); }
/// set_desired_alt - set desired altitude (in cm above home)
void set_desired_alt(float desired_alt) { _target.z = desired_alt; }
/// set_althold_kP - pass in alt hold controller's P gain
void set_althold_kP(float kP) { if(kP>0.0f) _althold_kP = kP; }
void set_desired_alt(float desired_alt) { _pos_control.set_alt_target(desired_alt); }
/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
void set_horizontal_velocity(float velocity_cms) { _wp_speed_cms = velocity_cms; };
@ -134,14 +134,11 @@ public:
/// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive
float get_descent_velocity() const { return _wp_speed_down_cms; };
/// get_waypoint_radius - access for waypoint radius in cm
float get_waypoint_radius() const { return _wp_radius_cm; }
/// get_wp_radius - access for waypoint radius in cm
float get_wp_radius() const { return _wp_radius_cm; }
/// get_waypoint_acceleration - returns acceleration in cm/s/s during missions
float get_waypoint_acceleration() const { return _wp_accel_cms.get(); }
/// set_lean_angle_max - limits maximum lean angle
void set_lean_angle_max(int16_t angle_cd) { if (angle_cd >= 1000 && angle_cd <= 8000) {_lean_angle_max_cd = angle_cd;} }
/// get_wp_acceleration - returns acceleration in cm/s/s during missions
float get_wp_acceleration() const { return _wp_accel_cms.get(); }
static const struct AP_Param::GroupInfo var_info[];
@ -152,30 +149,13 @@ protected:
uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
} _flags;
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void translate_loiter_target_movements(float nav_dt);
/// get_loiter_position_to_velocity - loiter position controller
/// converts desired position held in _target vector to desired velocity
void get_loiter_position_to_velocity(float dt, float max_speed_cms);
/// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void get_loiter_velocity_to_acceleration(float vel_lat_cms, float vel_lon_cms, float dt);
/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void get_loiter_acceleration_to_lean_angles(float accel_lat_cmss, float accel_lon_cmss);
/// 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 calc_loiter_desired_velocity(float nav_dt);
/// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
/// reset_I - clears I terms from loiter PID controller
void reset_I();
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
/// set climb param to true if track climbs vertically, false if descending
void calculate_wp_leash_length(bool climb);
@ -198,29 +178,19 @@ protected:
AP_Float _wp_speed_down_cms; // descent speed target in cm/s
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions
uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration
uint8_t _wpnav_step; // used to decide which portion of wpnav controller to run during this iteration
uint32_t _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call
float _loiter_dt; // time difference since last loiter call
float _wpnav_dt; // time difference since last loiter call
float _althold_kP; // alt hold's P gain
// output from controller
int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
// loiter controller internal variables
Vector3f _target; // loiter's target location in cm from home
int16_t _pilot_vel_forward_cms; // pilot's desired velocity forward (body-frame)
int16_t _pilot_vel_right_cms; // pilot's desired velocity right (body-frame)
Vector3f _target_vel; // pilot's latest desired velocity in earth-frame
Vector3f _vel_last; // previous iterations velocity in cm/s
uint32_t _loiter_last_update; // time of last update_loiter call
uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration
int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame)
int16_t _pilot_accel_rgt_cms; // pilot's desired acceleration right (body-frame)
float _loiter_leash; // loiter's horizontal leash length in cm. used to stop the pilot from pushing the target location too far from the current location
float _loiter_accel_cms; // loiter's acceleration in cm/s/s
int16_t _lean_angle_max_cd; // maximum lean angle in centi-degrees
// waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call
uint8_t _wp_step; // used to decide which portion of wpnav controller to run during this iteration
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector3f _destination; // target destination in cm from home (equivalent to next_WP)
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
@ -232,17 +202,5 @@ protected:
float _track_accel; // acceleration along track
float _track_speed; // speed in cm/s along track
float _track_leash_length; // leash length along track
public:
// for logging purposes
Vector2f dist_error; // distance error calculated by loiter controller
Vector2f desired_vel; // loiter controller desired velocity
Vector2f desired_accel; // the resulting desired acceleration
// To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation)
/// update - run the loiter and wpnav controllers - should be called at 100hz
//void update_100hz(void);
/// update - run the loiter and wpnav controllers - should be called at 10hz
//void update_10hz(void);
};
#endif // AC_WPNAV_H