zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_WPNav: improved comments 

Made SPEEDZ a parameter although it's not yet used
master
Randy Mackay 12 years ago
parent
fa493a0fb3
commit
effd5b0da7
2 changed files with 35 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 25
      libraries/AC_WPNav/AC_WPNav.cpp
  2. 35
      libraries/AC_WPNav/AC_WPNav.h

25
libraries/AC_WPNav/AC_WPNav.cpp
Unescape View File

@ -8,11 +8,11 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = { @@ -8,11 +8,11 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix
// @Param: SPEED
// @DisplayName: Waypoint Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain during a WP mission
// @DisplayName: Waypoint Horizontal Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission
// @Units: Centimeters/Second
// @Range: 0 1000
// @Increment: 100
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_cms, WPNAV_WP_SPEED),
@ -25,6 +25,16 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = { @@ -25,6 +25,16 @@ 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
// @Units: Centimeters/Second
// @Range: 0 1000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("SPEEDZ", 2, AC_WPNav, _speedz_cms, WPNAV_WP_SPEEDZ),
AP_GROUPEND
};
@ -38,7 +48,6 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo @@ -38,7 +48,6 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
_pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon),
_speedz_cms(MAX_CLIMB_VELOCITY),
_lean_angle_max(MAX_LEAN_ANGLE)
{
AP_Param::setup_object_defaults(this, var_info);
@ -102,7 +111,7 @@ void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float @@ -102,7 +111,7 @@ void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void AC_WPNav::translate_loiter_target_movements(float nav_dt)
{
Vector2f target_vel_adj; // make 2d vector?
Vector2f target_vel_adj;
float vel_delta_total;
float vel_max;
float vel_total;
@ -208,7 +217,7 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f @@ -208,7 +217,7 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
_origin = origin;
_destination = destination;
_vert_track_scale = WPINAV_MAX_POS_ERROR / WPINAV_MAX_ALT_ERROR;
_vert_track_scale = WPNAV_LEASH_XY / WPNAV_LEASH_Z;
Vector3f pos_delta = _destination - _origin;
pos_delta.z = pos_delta.z * _vert_track_scale;
_track_length = pos_delta.length();
@ -237,10 +246,10 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt) @@ -237,10 +246,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(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - track_error*track_error);
track_extra_max = safe_sqrt(WPNAV_LEASH_XY*WPNAV_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(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - (curr_delta_length*curr_delta_length - track_covered*track_covered));
// track_extra_max = safe_sqrt(WPNAV_LEASH_XY*WPNAV_LEASH_XY - (curr_delta_length*curr_delta_length - track_covered*track_covered));
track_desired_max = track_covered + track_extra_max;

35
libraries/AC_WPNav/AC_WPNav.h
Unescape View File

@ -29,22 +29,22 @@ @@ -29,22 +29,22 @@
#define WPNAV_WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
#define WPNAV_WP_RADIUS 200 // default waypoint radius in cm
#define WPINAV_MAX_POS_ERROR 531.25f // maximum distance (in cm) that the desired track can stray from our current location.
#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
// WPINAV_MAX_POS_ERROR = D0 + WP_SPEED.^2 ./ (2*MAX_LOITER_POS_ACCEL);
// WPNAV_LEASH_XY = D0 + WP_SPEED.^2 ./ (2*MAX_LOITER_POS_ACCEL);
// else
// WPINAV_MAX_POS_ERROR = min(200, WP_SPEED/Pid_P); // to stop it being over sensitive to error
// 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 MAX_CLIMB_VELOCITY 125 // maximum climb velocity - ToDo: pull this in from main code
#define WPINAV_MAX_ALT_ERROR 100.0f // maximum distance (in cm) that the desired track can stray from our current location.
#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
// WPINAV_MAX_ALT_ERROR = D0 + MAX_CLIMB_VELOCITY.^2 ./ (2*ALT_HOLD_ACCEL_MAX);
// WPNAV_LEASH_Z = D0 + WPNAV_WP_SPEEDZ.^2 ./ (2*ALT_HOLD_ACCEL_MAX);
// else
// WPINAV_MAX_ALT_ERROR = min(100, MAX_CLIMB_VELOCITY/Pid_P); // to stop it being over sensitive to error
// WPNAV_LEASH_Z = min(100, WPNAV_WP_SPEEDZ/Pid_P); // to stop it being over sensitive to error
// end
class AC_WPNav
{
@ -183,8 +183,8 @@ protected: @@ -183,8 +183,8 @@ protected:
AC_PID* _pid_rate_lon;
// parameters
AP_Float _speed_cms; // default horizontal speed in cm/s
float _speedz_cms; // max vertical climb rate in cm/s. To-Do: rename or pull this from main code
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
uint32_t _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call
@ -196,25 +196,24 @@ protected: @@ -196,25 +196,24 @@ protected:
int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
int32_t _lean_angle_max; // maximum lean angle. can we set from main code so that throttle controller can stop leans that cause copter to lose altitude
// internal variables
// loiter controller internal variables
Vector3f _target; // loiter's target location in cm from home
Vector3f _target_vel; // loiter
int16_t _pilot_vel_forward_cms; // pilot's desired velocity forward (body-frame)
int16_t _pilot_vel_right_cms; // pilot's desired velocity right (body-frame)
Vector3f _target_vel; // pilot's latest desired velocity in earth-frame
Vector3f _vel_last; // previous iterations velocity in cm/s
int32_t _lean_angle_max; // maximum lean angle. can be set from main code so that throttle controller can stop leans that cause copter to lose altitude
// waypoint controller internal variables
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector3f _destination; // target destination in cm from home (equivalent to next_WP)
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 _distance_to_target; // distance to loiter target
float _vert_track_scale; // vertical scaling to give altitude equal weighting to position
float _vert_track_scale; // vertical scaling to give altitude equal weighting to horizontal position
bool _reached_destination; // true if we have reached the destination
// pilot inputs for loiter
int16_t _pilot_vel_forward_cms;
int16_t _pilot_vel_right_cms;
public:
// for logging purposes
Vector2f dist_error; // distance error calculated by loiter controller

Write Preview
Loading…
Cancel
Save