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

AC_WPNav: move leashes to AC_PosControl

Browse Source
This commit is contained in:
Randy Mackay 2014-01-24 12:27:26 +09:00 committed by Andrew Tridgell
parent de34359808
commit 1596d83d02
2 changed files with 80 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -81,23 +81,16 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro
_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),
_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.0),
_track_speed(0.0),
_track_leash_length(0.0)
{
AP_Param::setup_object_defaults(this, var_info);
// initialise leash lengths
calculate_wp_leash_length(true);
calculate_loiter_leash_length();
}
///
@ -113,10 +106,9 @@ void AC_WPNav::set_loiter_target(const Vector3f& position)
// initialise feed forward velocity to zero
_pos_control.set_desired_velocity(0,0);
// initialise pos controller speed, acceleration and leash length
// initialise pos controller speed and acceleration
_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;
@ -138,19 +130,12 @@ void AC_WPNav::init_loiter_target()
// 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;
}
/// 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()
{
_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)
{
@ -288,9 +273,15 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_pos_delta_unit = pos_delta/_track_length;
}
// initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_speed_z(_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z();
// 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);
calculate_wp_leash_length();
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
@ -336,8 +327,17 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
track_extra_max = min(_track_leash_length*(_wp_leash_z-track_error_z)/_wp_leash_z, _track_leash_length*(_wp_leash_xy-track_error_xy)/_wp_leash_xy);
track_extra_max = min(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
if(track_extra_max <0) {
track_desired_max = track_covered;
}else{
@ -445,6 +445,46 @@ void AC_WPNav::update_wpnav()
}
}
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_wp_leash_length()
{
// length of the unit direction vector in the horizontal
float pos_delta_unit_xy = sqrt(_pos_delta_unit.x*_pos_delta_unit.x+_pos_delta_unit.y*_pos_delta_unit.y);
float pos_delta_unit_z = fabsf(_pos_delta_unit.z);
float speed_z;
float leash_z;
if (_pos_delta_unit.z >= 0) {
speed_z = _wp_speed_up_cms;
leash_z = _pos_control.get_leash_up_z();
}else{
speed_z = _wp_speed_down_cms;
leash_z = _pos_control.get_leash_down_z();
}
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(pos_delta_unit_z == 0 && pos_delta_unit_xy == 0){
_track_accel = 0;
_track_speed = 0;
_track_leash_length = WPNAV_MIN_LEASH_LENGTH;
}else if(_pos_delta_unit.z == 0){
_track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
}else if(pos_delta_unit_xy == 0){
_track_accel = WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z;
_track_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_z/pos_delta_unit_z;
}else{
_track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_speed = min(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
_track_leash_length = min(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);
}
// debug -- remove me!
hal.console->printf_P(PSTR("\nTAC:%4.2f S:%4.2f L:%4.2f\n"),(float)_track_accel, (float)_track_speed, (float)_track_leash_length);
}
///
/// shared methods
///
@ -459,79 +499,3 @@ float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destinati
}
return bearing;
}
/// 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) {
_wp_accel_cms = WPNAV_ACCELERATION_MIN;
}
// avoid divide by zero
if (kP <= 0.0f) {
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
return;
}
// 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;
}else{
// leash length grows at sqrt of speed further out
_wp_leash_xy = (_wp_accel_cms / (2.0f*kP*kP)) + (_wp_speed_cms*_wp_speed_cms / (2.0f*_wp_accel_cms));
}
// ensure leash is at least 1m long
if( _wp_leash_xy < WPNAV_MIN_LEASH_LENGTH ) {
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
}
// calculate vertical leash length
float speed_vert;
if( climb ) {
speed_vert = _wp_speed_up_cms;
}else{
speed_vert = _wp_speed_down_cms;
}
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;
}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));
}
// ensure leash is at least 1m long
if( _wp_leash_z < WPNAV_MIN_LEASH_LENGTH ) {
_wp_leash_z = WPNAV_MIN_LEASH_LENGTH;
}
// length of the unit direction vector in the horizontal
float pos_delta_unit_xy = sqrt(_pos_delta_unit.x*_pos_delta_unit.x+_pos_delta_unit.y*_pos_delta_unit.y);
float pos_delta_unit_z = fabsf(_pos_delta_unit.z);
// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel
if(pos_delta_unit_z == 0 && pos_delta_unit_xy == 0){
_track_accel = 0;
_track_speed = 0;
_track_leash_length = WPNAV_MIN_LEASH_LENGTH;
}else if(_pos_delta_unit.z == 0){
_track_accel = _wp_accel_cms/pos_delta_unit_xy;
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _wp_leash_xy/pos_delta_unit_xy;
}else if(pos_delta_unit_xy == 0){
_track_accel = WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z;
_track_speed = speed_vert/pos_delta_unit_z;
_track_leash_length = _wp_leash_z/pos_delta_unit_z;
}else{
_track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_speed = min(speed_vert/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
_track_leash_length = min(_wp_leash_z/pos_delta_unit_z, _wp_leash_xy/pos_delta_unit_xy);
}
}

46
libraries/AC_WPNav/AC_WPNav.h
View File

@ -77,6 +77,24 @@ public:
/// waypoint controller
///
/// 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; };
/// get_horizontal_velocity - allows main code to retrieve target horizontal velocity for wp navigation
float get_horizontal_velocity() { return _wp_speed_cms; };
/// get_climb_velocity - returns target climb speed in cm/s during missions
float get_climb_velocity() const { return _wp_speed_up_cms; };
/// 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_wp_radius - access for waypoint radius in cm
float get_wp_radius() const { return _wp_radius_cm; }
/// get_wp_acceleration - returns acceleration in cm/s/s during missions
float get_wp_acceleration() const { return _wp_accel_cms.get(); }
/// get_wp_destination waypoint using position vector (distance from home in cm)
const Vector3f &get_wp_destination() const { return _destination; }
@ -105,6 +123,9 @@ public:
/// update_wp - update waypoint controller
void update_wpnav();
/// calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller
void calculate_wp_leash_length();
///
/// shared methods
///
@ -119,24 +140,6 @@ public:
/// set_desired_alt - set desired altitude (in cm above home)
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; };
/// get_horizontal_velocity - allows main code to retrieve target horizontal velocity for wp navigation
float get_horizontal_velocity() { return _wp_speed_cms; };
/// get_climb_velocity - returns target climb speed in cm/s during missions
float get_climb_velocity() const { return _wp_speed_up_cms; };
/// 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_wp_radius - access for waypoint radius in cm
float get_wp_radius() const { return _wp_radius_cm; }
/// get_wp_acceleration - returns acceleration in cm/s/s during missions
float get_wp_acceleration() const { return _wp_accel_cms.get(); }
/// advance_wp_target_along_track - move target location along track from origin to destination
void advance_wp_target_along_track(float dt);
@ -156,10 +159,6 @@ protected:
/// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
/// 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);
// references to inertial nav and ahrs libraries
const AP_InertialNav* const _inav;
const AP_AHRS* const _ahrs;
@ -185,7 +184,6 @@ 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)
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
// waypoint controller internal variables
@ -196,8 +194,6 @@ protected:
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
float _track_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm
float _wp_leash_xy; // horizontal leash length in cm
float _wp_leash_z; // horizontal leash length in cm
float _limited_speed_xy_cms; // horizontal speed in cm/s used to advance the intermediate target towards the destination. used to limit extreme acceleration after passing a waypoint
float _track_accel; // acceleration along track
float _track_speed; // speed in cm/s along track