From b38c4848741f2a5b4d63b9aef7d146dcf4acabee Mon Sep 17 00:00:00 2001 From: lthall Date: Tue, 6 May 2014 16:41:54 +0900 Subject: [PATCH] AC_WPNav: add LOIT_JERK parameter Limit accel output from loiter controller. Call new pos_control.init_xy_controller when loiter starts Remove sudden stop when pilot requested acceleration is zero Pair programmed with Randy --- libraries/AC_WPNav/AC_WPNav.cpp | 74 ++++++++++++++++++++++----------- libraries/AC_WPNav/AC_WPNav.h | 5 +++ 2 files changed, 55 insertions(+), 24 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 038a3a5dd9..b4a7ae4180 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -70,6 +70,15 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT), + // @Param: LOIT_JERK + // @DisplayName: Loiter maximum jerk + // @Description: Loiter maximum jerk in cm/s/s/s + // @Units: cm/s/s/s + // @Range: 500 2000 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("LOIT_JERK", 7, AC_WPNav, _loiter_jerk_max_cmsss, WPNAV_LOITER_JERK_MAX_DEFAULT), + AP_GROUPEND }; @@ -134,7 +143,11 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I) /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity void AC_WPNav::init_loiter_target() { - Vector3f curr_vel = _inav.get_velocity(); + const Vector3f& curr_pos = _inav.get_position(); + const Vector3f& curr_vel = _inav.get_velocity(); + + // initialise position controller + _pos_control.init_xy_controller(); // initialise pos controller speed and acceleration _pos_control.set_speed_xy(_loiter_speed_cms); @@ -142,11 +155,15 @@ void AC_WPNav::init_loiter_target() _pos_control.set_accel_xy(_loiter_accel_cms); // set target position - _pos_control.set_target_to_stopping_point_xy(); + _pos_control.set_xy_target(curr_pos.x, curr_pos.y); - // initialise feed forward velocities to zero + // move current vehicle velocity into feed forward velocity _pos_control.set_desired_velocity(curr_vel.x, curr_vel.y); + // initialise desired accel and add fake wind + _loiter_desired_accel.x = (_loiter_accel_cms)*curr_vel.x/_loiter_speed_cms; + _loiter_desired_accel.y = (_loiter_accel_cms)*curr_vel.y/_loiter_speed_cms; + // initialise pilot input _pilot_accel_fwd_cms = 0; _pilot_accel_rgt_cms = 0; @@ -202,33 +219,42 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt) 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()); + // calculate the difference + Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel); + + // constrain and scale the desired acceleration + float des_accel_change_total = pythagorous2(des_accel_diff.x, des_accel_diff.y); + float accel_change_max = _loiter_jerk_max_cmsss * nav_dt; + if (des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) { + des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total; + des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total; + } + // adjust the desired acceleration + _loiter_desired_accel += des_accel_diff; + // 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; + desired_vel += _loiter_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 = pythagorous2(desired_vel.x, desired_vel.y); - if (vel_total > _loiter_speed_cms && vel_total > 0.0f) { - desired_vel.x = _loiter_speed_cms * desired_vel.x/vel_total; - desired_vel.y = _loiter_speed_cms * desired_vel.y/vel_total; + if (_pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f) { + desired_vel.x -= (_loiter_accel_cms)*nav_dt*desired_vel.x/_loiter_speed_cms; + desired_vel.y -= (_loiter_accel_cms)*nav_dt*desired_vel.y/_loiter_speed_cms; + } else { + desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms; + if(desired_vel.x > 0 ) { + desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); + }else if(desired_vel.x < 0) { + desired_vel.x = min(desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); + } + desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms; + if(desired_vel.y > 0 ) { + desired_vel.y = max(desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); + }else if(desired_vel.y < 0) { + desired_vel.y = min(desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); + } } // send adjusted feed forward velocity back to position controller diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 46b2d9720e..081711bd6e 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -16,6 +16,7 @@ #define WPNAV_LOITER_SPEED_MIN 100.0f // minimum loiter speed in cm/s #define WPNAV_LOITER_ACCEL 250.0f // default acceleration in loiter mode #define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum acceleration in loiter mode +#define WPNAV_LOITER_JERK_MAX_DEFAULT 500.0f // maximum jerk in cm/s/s/s in loiter mode #define WPNAV_WP_SPEED 500.0f // default horizontal speed betwen waypoints in cm/s #define WPNAV_WP_SPEED_MIN 100.0f // minimum horizontal speed between waypoints in cm/s @@ -37,6 +38,8 @@ # define WPNAV_WP_UPDATE_TIME 0.095f // 10hz update rate on low speed CPUs (APM1, APM2) #endif +#define WPNAV_LOITER_ACTIVE_TIMEOUT_MS 200 // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds) + class AC_WPNav { public: @@ -250,6 +253,7 @@ protected: // parameters AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter + AP_Float _loiter_jerk_max_cmsss; // maximum jerk in cm/s/s/s while in loiter AP_Float _wp_speed_cms; // maximum horizontal speed in cm/s during missions AP_Float _wp_speed_up_cms; // climb speed target in cm/s AP_Float _wp_speed_down_cms; // descent speed target in cm/s @@ -262,6 +266,7 @@ protected: 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) + Vector2f _loiter_desired_accel; // slewed pilot's desired acceleration in lat/lon frame float _loiter_accel_cms; // loiter's acceleration in cm/s/s // waypoint controller internal variables