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

AC_WPNav: update target speed immediately

Browse Source
This commit is contained in:
Randy Mackay 2014-04-21 14:48:56 +09:00
parent 05bffb5915
commit 2167dd7d3e
2 changed files with 35 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -140,6 +140,22 @@ void AC_WPNav::init_loiter_target()
_pilot_accel_rgt_cms = 0;
}
/// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
void AC_WPNav::set_loiter_velocity(float velocity_cms)
{
// range check velocity and update position controller
if (velocity_cms >= WPNAV_LOITER_SPEED_MIN) {
_loiter_speed_cms = velocity_cms;
// initialise pos controller speed
_pos_control.set_speed_xy(_loiter_speed_cms);
// initialise pos controller acceleration
_loiter_accel_cms = _loiter_speed_cms/2.0f;
_pos_control.set_accel_xy(_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)
{
@ -164,8 +180,8 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
}
// check loiter speed and avoid divide by zero
if( _loiter_speed_cms < 100.0f) {
_loiter_speed_cms = 100.0f;
if( _loiter_speed_cms < WPNAV_LOITER_SPEED_MIN) {
_loiter_speed_cms = WPNAV_LOITER_SPEED_MIN;
_loiter_accel_cms = _loiter_speed_cms/2.0f;
}
@ -243,6 +259,16 @@ void AC_WPNav::update_loiter()
/// waypoint navigation
///
/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
void AC_WPNav::set_horizontal_velocity(float velocity_cms)
{
// range check new target speed and update position controller
if (_wp_speed_cms >= WPNAV_WP_SPEED_MIN) {
_wp_speed_cms = velocity_cms;
_pos_control.set_speed_xy(_wp_speed_cms);
}
}
/// set_destination - set destination using cm from home
void AC_WPNav::set_wp_destination(const Vector3f& destination)
{

12
libraries/AC_WPNav/AC_WPNav.h
View File

@ -16,6 +16,7 @@
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
#define WPNAV_LOITER_SPEED 500.0f // maximum default loiter speed in cm/s
#define WPNAV_LOITER_SPEED_MIN 100.0f // minimum loiter speed in cm/s
#define WPNAV_LOITER_ACCEL_MAX 250.0f // maximum acceleration in loiter mode
#define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum acceleration in loiter mode
#define WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR 200.0f // maximum speed used to correct position error (i.e. not including feed forward)
@ -23,6 +24,7 @@
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle
#define WPNAV_WP_SPEED 500.0f // default horizontal speed betwen waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 100.0f // minimum horitzontal speed between 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
@ -66,7 +68,7 @@ public:
void init_loiter_target();
/// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
void set_loiter_velocity(float velocity_cms) { _loiter_speed_cms = velocity_cms; };
void set_loiter_velocity(float velocity_cms);
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
@ -91,16 +93,16 @@ public:
///
/// 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; };
void set_horizontal_velocity(float velocity_cms);
/// get_horizontal_velocity - allows main code to retrieve target horizontal velocity for wp navigation
float get_horizontal_velocity() { return _wp_speed_cms; };
float get_horizontal_velocity() const { 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; };
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; };
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; }