From 0c8cbba6445f01f95a54539e98a6d40207a9910c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 19 Jan 2014 19:26:29 +0900 Subject: [PATCH] AC_WPNav: remove xy pos controller --- libraries/AC_WPNav/AC_WPNav.cpp | 534 +++++++++----------------------- libraries/AC_WPNav/AC_WPNav.h | 150 ++++----- 2 files changed, 203 insertions(+), 481 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 4947c9b3af..3920de8779 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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 diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index e211c4a36b..641925933e 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -2,7 +2,6 @@ #ifndef AC_WPNAV_H #define AC_WPNAV_H -#include #include #include #include @@ -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