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

AC_WPNav: add LOIT_JERK parameter

Browse Source 
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
This commit is contained in:
lthall 2014-05-06 16:41:54 +09:00 committed by Randy Mackay
parent 71fae1e6f2
commit b38c484874
2 changed files with 55 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -70,6 +70,15 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// @User: Standard // @User: Standard
AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT), 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 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 /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::init_loiter_target() 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 // initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms); _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); _pos_control.set_accel_xy(_loiter_accel_cms);
// set target position // 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); _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 // initialise pilot input
_pilot_accel_fwd_cms = 0; _pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_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.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()); 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 // get pos_control's feed forward velocity
Vector2f desired_vel = _pos_control.get_desired_velocity(); Vector2f desired_vel = _pos_control.get_desired_velocity();
// add pilot commanded acceleration // add pilot commanded acceleration
desired_vel += desired_accel * nav_dt; desired_vel += _loiter_desired_accel * nav_dt;
// reduce velocity with fake wind resistance // reduce velocity with fake wind resistance
if(desired_vel.x > 0 ) { if (_pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f) {
desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms; desired_vel.x -= (_loiter_accel_cms)*nav_dt*desired_vel.x/_loiter_speed_cms;
desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); desired_vel.y -= (_loiter_accel_cms)*nav_dt*desired_vel.y/_loiter_speed_cms;
}else if(desired_vel.x < 0) { } else {
desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms; 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.x > 0 ) {
} desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
if(desired_vel.y > 0 ) { }else if(desired_vel.x < 0) {
desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms; desired_vel.x = min(desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
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 -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms; if(desired_vel.y > 0 ) {
desired_vel.y = min(desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 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);
// 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;
} }
// send adjusted feed forward velocity back to position controller // send adjusted feed forward velocity back to position controller

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -16,6 +16,7 @@
#define WPNAV_LOITER_SPEED_MIN 100.0f // minimum loiter speed in cm/s #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 250.0f // default acceleration in loiter mode
#define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum 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 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 #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) # define WPNAV_WP_UPDATE_TIME 0.095f // 10hz update rate on low speed CPUs (APM1, APM2)
#endif #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 class AC_WPNav
{ {
public: public:
@ -250,6 +253,7 @@ protected:
// parameters // parameters
AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter 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_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_up_cms; // climb speed target in cm/s
AP_Float _wp_speed_down_cms; // descent 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 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_fwd_cms; // pilot's desired acceleration forward (body-frame)
int16_t _pilot_accel_rgt_cms; // pilot's desired acceleration right (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 float _loiter_accel_cms; // loiter's acceleration in cm/s/s
// waypoint controller internal variables // waypoint controller internal variables