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

AC_WPNav: add climb and descent params, dynamic leash length

Browse Source
This commit is contained in:
Randy Mackay 2013-04-18 14:51:01 +09:00
parent effd5b0da7
commit 764853bd56
2 changed files with 93 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -14,7 +14,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// @Range: 0 1000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_cms, WPNAV_WP_SPEED),
AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_xy_cms, WPNAV_WP_SPEED),
// @Param: RADIUS
// @DisplayName: Waypoint Radius
@ -25,15 +25,23 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// @User: Standard
AP_GROUPINFO("RADIUS", 1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS),
// @Param: SPEEDZ
// @DisplayName: Waypoint Vertical Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain vertically during a WP mission
// @Param: SPEED_UP
// @DisplayName: Waypoint Climb Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while climbing during a WP mission
// @Units: Centimeters/Second
// @Range: 0 1000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEEDZ", 2, AC_WPNav, _speedz_cms, WPNAV_WP_SPEEDZ),
AP_GROUPINFO("SPEED_UP", 2, AC_WPNav, _speed_up_cms, WPNAV_WP_SPEED_UP),
// @Param: SPEED_DN
// @DisplayName: Waypoint Descent Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while descending during a WP mission
// @Units: Centimeters/Second
// @Range: 0 1000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEED_DN", 3, AC_WPNav, _speed_down_cms, WPNAV_WP_SPEED_DOWN),
AP_GROUPEND
};
@ -216,9 +224,11 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
{
_origin = origin;
_destination = destination;
_vert_track_scale = WPNAV_LEASH_XY / WPNAV_LEASH_Z;
Vector3f pos_delta = _destination - _origin;
bool climb = pos_delta.z >= 0; // climb vs descending lead to different leash lengths because speed_up_cms and speed_down_cms can be different
calculate_leash_length(climb); // update leash lengths and _vert_track_scale
pos_delta.z = pos_delta.z * _vert_track_scale;
_track_length = pos_delta.length();
_pos_delta_unit = pos_delta/_track_length;
@ -246,10 +256,10 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
track_error = safe_sqrt(curr_delta_length*curr_delta_length - track_covered*track_covered);
track_extra_max = safe_sqrt(WPNAV_LEASH_XY*WPNAV_LEASH_XY - track_error*track_error);
track_extra_max = safe_sqrt(_leash_xy*_leash_xy - track_error*track_error);
// we could save a sqrt by doing the following and not assigning track_error
// track_extra_max = safe_sqrt(WPNAV_LEASH_XY*WPNAV_LEASH_XY - (curr_delta_length*curr_delta_length - track_covered*track_covered));
// track_extra_max = safe_sqrt(_leash_xy*_leash_xy - (curr_delta_length*curr_delta_length - track_covered*track_covered));
track_desired_max = track_covered + track_extra_max;
@ -308,7 +318,7 @@ void AC_WPNav::update_wpnav()
reset_I();
}else{
// advance the target if necessary
advance_target_along_track(_speed_cms, dt);
advance_target_along_track(_speed_xy_cms, dt);
}
// run loiter position controller
@ -443,4 +453,52 @@ void AC_WPNav::reset_I()
// reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0;
_target_vel.y = 0;
}
/// calculate_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_leash_length(bool climb)
{
// get loiter position P
float kP = _pid_pos_lat->kP();
if(kP <= 0.51) {
kP = 0.51;
}
// calculate horiztonal leash length
if(_speed_xy_cms <= MAX_LOITER_POS_ACCEL / kP) {
// linear leash length based on speed close in
_leash_xy = _speed_xy_cms / kP;
}else{
// leash length grows at sqrt of speed further out
_leash_xy = (MAX_LOITER_POS_ACCEL / (2.0*kP*kP)) + (_speed_xy_cms*_speed_xy_cms / (2*MAX_LOITER_POS_ACCEL));
}
// ensure leash is at least 1m long
if( _leash_xy < 100 ) {
_leash_xy = 100;
}
// calculate vertical leash length
float speed_vert, leash_z;
if( climb ) {
speed_vert = _speed_up_cms;
}else{
speed_vert = _speed_down_cms;
}
if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / WPNAV_ALT_HOLD_P) {
// linear leash length based on speed close in
leash_z = speed_vert / WPNAV_ALT_HOLD_P;
}else{
// leash length grows at sqrt of speed further out
leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*WPNAV_ALT_HOLD_P*WPNAV_ALT_HOLD_P)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX));
}
// ensure leash is at least 1m long
if( leash_z < 100 ) {
leash_z = 100;
}
// calculate vertical track scale used to give altitude equal weighting to horizontal position
_vert_track_scale = _leash_xy / leash_z;
}

51
libraries/AC_WPNav/AC_WPNav.h
View File

@ -27,25 +27,15 @@
// MAX_LOITER_OVERSHOOT = min(200, MAX_LOITER_POS_VELOCITY/Pid_P); // to stop it being over sensitive to error
// end
#define WPNAV_WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
#define WPNAV_WP_RADIUS 200 // default waypoint radius in cm
#define WPNAV_LEASH_XY 531.25f // maximum distance (in cm) that the desired track can stray from our current location.
// D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
// if WP_SPEED > 2*D0*Pid_P
// WPNAV_LEASH_XY = D0 + WP_SPEED.^2 ./ (2*MAX_LOITER_POS_ACCEL);
// else
// WPNAV_LEASH_XY = min(200, WP_SPEED/Pid_P); // to stop it being over sensitive to error
// end
// This should use the current waypoint max speed though rather than the default
#define WPNAV_WP_SPEED 500.0f // default horizontal speed betwen waypoints in cm/s
#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm
#define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity
#define WPNAV_WP_SPEED_DOWN 125.0f // default maximum descent velocity
#define WPNAV_ALT_HOLD_P 2.0f // hard coded estimate of throttle controller's altitude hold's P gain. To-Do: retrieve gain from throttle controller
#define WPNAV_ALT_HOLD_ACCEL_MAX 250.0f // hard coded estimate of throttle controller's maximum acceleration in cm/s. To-Do: retrieve from throttle controller
#define WPNAV_WP_SPEEDZ 125 // maximum climb velocity - ToDo: pull this in from main code
#define WPNAV_LEASH_Z 100.0f // maximum distance (in cm) that the desired track can stray from our current location.
// D0 = ALT_HOLD_ACCEL_MAX/(2*Pid_P^2);
// if g.pilot_velocity_z_max > 2*D0*Pid_P
// WPNAV_LEASH_Z = D0 + WPNAV_WP_SPEEDZ.^2 ./ (2*ALT_HOLD_ACCEL_MAX);
// else
// WPNAV_LEASH_Z = min(100, WPNAV_WP_SPEEDZ/Pid_P); // to stop it being over sensitive to error
// end
class AC_WPNav
{
public:
@ -137,10 +127,13 @@ public:
}
/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
void set_horizontal_velocity(float velocity_cms) { _speed_cms = velocity_cms; };
void set_horizontal_velocity(float velocity_cms) { _speed_xy_cms = velocity_cms; };
/// set_climb_velocity - allows main code to pass max climb velocity to wp navigation
void set_climb_velocity(float velocity_cms) { _speedz_cms = velocity_cms; };
/// get_climb_velocity - returns target climb speed in cm/s during missions
float get_climb_velocity() { return _speed_up_cms; };
/// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive
float get_descent_velocity() { return _speed_down_cms; };
/// get_waypoint_radius - access for waypoint radius in cm
float get_waypoint_radius() { return _wp_radius_cm; }
@ -173,6 +166,10 @@ protected:
/// reset_I - clears I terms from loiter PID controller
void reset_I();
/// calculate_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_leash_length(bool climb);
// pointers to inertial nav library
AP_InertialNav* _inav;
@ -183,12 +180,13 @@ protected:
AC_PID* _pid_rate_lon;
// parameters
AP_Float _speed_cms; // horizontal speed target in cm/s
AP_Float _speedz_cms; // vertical 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 _speed_xy_cms; // horizontal speed target in cm/s
AP_Float _speed_up_cms; // climb speed target in cm/s
AP_Float _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
uint32_t _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw;
float _cos_roll;
@ -211,8 +209,9 @@ protected:
float _track_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm
float _distance_to_target; // distance to loiter target
float _vert_track_scale; // vertical scaling to give altitude equal weighting to horizontal position
bool _reached_destination; // true if we have reached the destination
float _vert_track_scale; // vertical scaling to give altitude equal weighting to horizontal position
float _leash_xy; // horizontal leash length in cm
public:
// for logging purposes