aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: limit max loiter position error

Browse Source 
move interpretation of pilot input to wpnav lib
This commit is contained in:
Randy Mackay 2013-04-02 23:22:09 +09:00
parent 31838b2865
commit 6dbcbdcb43
3 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -1682,7 +1682,7 @@ void update_roll_pitch_mode(void)
control_pitch = g.rc_2.control_in;
// update loiter target from user controls - max velocity is 5.0 m/s
wp_nav.move_loiter_target(-control_pitch/(2*4.5), control_roll/(2*4.5),0.01f);
wp_nav.move_loiter_target(control_roll, control_pitch,0.01f);
// copy latest output from nav controller to stabilize controller
nav_roll += constrain_int32(wrap_180_cd(wp_nav.get_desired_roll() - nav_roll), -g.auto_slew_rate.get(), g.auto_slew_rate.get()); // 40 deg a second

18
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -71,10 +71,11 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
void AC_WPNav::move_loiter_target(float vel_forward_cms, float vel_right_cms, float dt)
void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float dt)
{
_pilot_vel_forward_cms = vel_forward_cms;
_pilot_vel_right_cms = vel_right_cms;
// convert pilot input to desired velocity in cm/s
_pilot_vel_forward_cms = -control_pitch * MAX_LOITER_POS_VELOCITY / 4500.0f;
_pilot_vel_right_cms = control_roll * MAX_LOITER_POS_VELOCITY / 4500.0f;
}
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
@ -116,6 +117,15 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
// 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 > MAX_LOITER_OVERSHOOT ) {
_target.x = curr_pos.x + MAX_LOITER_OVERSHOOT * distance_err.x/distance;
_target.y = curr_pos.y + MAX_LOITER_OVERSHOOT * distance_err.y/distance;
}
}
/// get_distance_to_target - get horizontal distance to loiter target in cm
@ -147,7 +157,7 @@ void AC_WPNav::update_loiter()
// translate any adjustments from pilot to loiter target
translate_loiter_target_movements(dt);
// run loiter position controller
get_loiter_pos_lat_lon(_target.x, _target.y, dt);
}

12
libraries/AC_WPNav/AC_WPNav.h
View File

@ -13,9 +13,11 @@
#include <AP_InertialNav.h> // Inertial Navigation library
// loiter maximum velocities and accelerations
#define MAX_LOITER_POS_VELOCITY 750 // should be 1.5 ~ 2.0 times the pilot input's max velocity
#define MAX_LOITER_POS_ACCEL 250 // maximum acceleration in cm/s
#define MAX_LOITER_VEL_ACCEL 400 // should be 1.5 times larger than MAX_LOITER_POS_ACCEL
#define MAX_LOITER_POS_VELOCITY 500 // maximum velocity that our position controller will request. should be 1.5 ~ 2.0 times the pilot input's max velocity. To-Do: make consistent with maximum velocity requested by pilot input to loiter
#define MAX_LOITER_POS_ACCEL 250 // defines the velocity vs distant curve. maximum acceleration in cm/s/s that loiter position controller asks for from acceleration controller
#define MAX_LOITER_VEL_ACCEL 400 // max acceleration in cm/s that the loiter velocity controller will ask from the lower accel controller.
// should be 1.5 times larger than MAX_LOITER_POS_ACCEL.
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
#define MAX_LOITER_POS_VEL_VELOCITY 1000
#define MAX_LOITER_OVERSHOOT 1000 // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
#define WPINAV_MAX_POS_ERROR 2000.0f // maximum distance (in cm) that the desired track can stray from our current location.
@ -43,8 +45,8 @@ public:
/// set_loiter_target - set initial loiter target based on current position and velocity
void set_loiter_target(const Vector3f& position, const Vector3f& velocity);
/// move_loiter_target - move destination using forward and right velocities in cm/s
void move_loiter_target(float vel_forward_cms, float vel_right_cms, float dt);
/// 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();