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

AC_WPNav: improved comments

Browse Source 
Made SPEEDZ a parameter although it's not yet used
This commit is contained in:
Randy Mackay 2013-04-18 11:17:41 +09:00
parent fa493a0fb3
commit effd5b0da7
2 changed files with 35 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -8,11 +8,11 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix // index 0 was used for the old orientation matrix
// @Param: SPEED // @Param: SPEED
// @DisplayName: Waypoint Speed Target // @DisplayName: Waypoint Horizontal Speed Target
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain during a WP mission // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission
// @Units: Centimeters/Second // @Units: Centimeters/Second
// @Range: 0 1000 // @Range: 0 1000
// @Increment: 100 // @Increment: 50
// @User: Standard // @User: Standard
AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_cms, WPNAV_WP_SPEED), AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_cms, WPNAV_WP_SPEED),
@ -23,8 +23,18 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// @Range: 100 1000 // @Range: 100 1000
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("RADIUS", 1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS), 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 AP_GROUPEND
}; };
@ -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_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat), _pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon), _pid_rate_lon(pid_rate_lon),
_speedz_cms(MAX_CLIMB_VELOCITY),
_lean_angle_max(MAX_LEAN_ANGLE) _lean_angle_max(MAX_LEAN_ANGLE)
{ {
AP_Param::setup_object_defaults(this, var_info); 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
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void AC_WPNav::translate_loiter_target_movements(float nav_dt) 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_delta_total;
float vel_max; float vel_max;
float vel_total; float vel_total;
@ -208,7 +217,7 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
_origin = origin; _origin = origin;
_destination = destination; _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; Vector3f pos_delta = _destination - _origin;
pos_delta.z = pos_delta.z * _vert_track_scale; pos_delta.z = pos_delta.z * _vert_track_scale;
_track_length = pos_delta.length(); _track_length = pos_delta.length();
@ -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_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_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 // 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; track_desired_max = track_covered + track_extra_max;

35
libraries/AC_WPNav/AC_WPNav.h
View File

@ -29,22 +29,22 @@
#define WPNAV_WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s #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_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); // D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
// if WP_SPEED > 2*D0*Pid_P // 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 // 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 // end
// This should use the current waypoint max speed though rather than the default // 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 WPNAV_WP_SPEEDZ 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_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); // D0 = ALT_HOLD_ACCEL_MAX/(2*Pid_P^2);
// if g.pilot_velocity_z_max > 2*D0*Pid_P // 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 // 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 // end
class AC_WPNav class AC_WPNav
{ {
@ -183,8 +183,8 @@ protected:
AC_PID* _pid_rate_lon; AC_PID* _pid_rate_lon;
// parameters // parameters
AP_Float _speed_cms; // default horizontal speed in cm/s AP_Float _speed_cms; // horizontal speed target in cm/s
float _speedz_cms; // max vertical climb rate in cm/s. To-Do: rename or pull this from main code 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 _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 _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call uint32_t _wpnav_last_update; // time of last update_wpnav call
@ -196,25 +196,24 @@ protected:
int32_t _desired_roll; // fed to stabilize controllers at 50hz int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // 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 // loiter controller internal variables
// internal variables
Vector3f _target; // loiter's target location in cm from home 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 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 _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 _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 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_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm float _track_desired; // our desired distance along the track in cm
float _distance_to_target; // distance to loiter target 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 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: public:
// for logging purposes // for logging purposes
Vector2f dist_error; // distance error calculated by loiter controller Vector2f dist_error; // distance error calculated by loiter controller