Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: remove loiter

Browse Source 
Loiter is in separate AC_Loiter class
This commit is contained in:
Randy Mackay 2018-03-28 11:09:13 +09:00
parent 0ba22a1feb
commit 9426ee6df6
2 changed files with 1 additions and 353 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

280
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -42,15 +42,6 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("SPEED_DN", 3, AC_WPNav, _wp_speed_down_cms, WPNAV_WP_SPEED_DOWN), AP_GROUPINFO("SPEED_DN", 3, AC_WPNav, _wp_speed_down_cms, WPNAV_WP_SPEED_DOWN),
// @Param: LOIT_SPEED
// @DisplayName: Loiter Horizontal Maximum Speed
// @Description: Defines the maximum speed in cm/s which the aircraft will travel horizontally while in loiter mode
// @Units: cm/s
// @Range: 20 2000
// @Increment: 50
// @User: Standard
AP_GROUPINFO("LOIT_SPEED", 4, AC_WPNav, _loiter_speed_cms, WPNAV_LOITER_SPEED),
// @Param: ACCEL // @Param: ACCEL
// @DisplayName: Waypoint Acceleration // @DisplayName: Waypoint Acceleration
// @Description: Defines the horizontal acceleration in cm/s/s used during missions // @Description: Defines the horizontal acceleration in cm/s/s used during missions
@ -69,33 +60,6 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT), AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT),
// @Param: BRK_JERK
// @DisplayName: Loiter braking jerk
// @Description: Loiter braking jerk in cm/s/s/s. Higher values will remove braking faster if the pilot moves the sticks during a braking manuver.
// @Units: cm/s/s/s
// @Range: 500 5000
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("BRK_JERK", 7, AC_WPNav, _loiter_brake_jerk_max_cmsss, WPNAV_LOITER_BRAKE_JERK),
// @Param: LOIT_MAXA
// @DisplayName: Loiter maximum correction acceleration
// @Description: Loiter maximum correction acceleration in cm/s/s. Higher values cause the copter to correct position errors more aggressively.
// @Units: cm/s/s
// @Range: 100 981
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOIT_MAXA", 8, AC_WPNav, _loiter_accel_cmss, WPNAV_LOITER_ACCEL_MAX),
// @Param: BRK_ACCEL
// @DisplayName: Loiter braking acceleration
// @Description: Loiter braking acceleration in cm/s/s. Higher values stop the copter more quickly when the stick is centered.
// @Units: cm/s/s
// @Range: 25 250
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("BRK_ACCEL", 9, AC_WPNav, _loiter_brake_accel_cmss, WPNAV_LOITER_BRAKE_ACCEL),
// @Param: RFND_USE // @Param: RFND_USE
// @DisplayName: Waypoint missions use rangefinder for terrain following // @DisplayName: Waypoint missions use rangefinder for terrain following
// @Description: This controls if waypoint missions use rangefinder for terrain following // @Description: This controls if waypoint missions use rangefinder for terrain following
@ -103,24 +67,6 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1), AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1),
// @Param: BRK_DELAY
// @DisplayName: Loiter brake start delay (in seconds)
// @Description: Loiter brake start delay (in seconds)
// @Units: s
// @Range: 0 2
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("BRK_DELAY", 11, AC_WPNav, _loiter_brake_delay, WPNAV_LOITER_BRAKE_START_DELAY),
// @Param: LOIT_ANGM
// @DisplayName: Loiter Angle Max
// @Description: Loiter maximum lean angle. Set to zero for 2/3 of PSC_ANGLE_MAX or ANGLE_MAX
// @Units: deg
// @Range: 0 45
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOIT_ANGM", 12, AC_WPNav, _loiter_angle_max, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -159,236 +105,10 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
_flags.segment_type = SEGMENT_STRAIGHT; _flags.segment_type = SEGMENT_STRAIGHT;
// sanity check some parameters // sanity check some parameters
_loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN);
_loiter_accel_cmss = MIN(_loiter_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_accel_cms = MIN(_wp_accel_cms, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); _wp_accel_cms = MIN(_wp_accel_cms, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN); _wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN);
} }
///
/// loiter controller
///
/// init_loiter_target in cm from home
void AC_WPNav::init_loiter_target(const Vector3f& position)
{
// initialise pos controller speed, acceleration
_pos_control.set_speed_xy(WPNAV_LOITER_VEL_CORRECTION_MAX);
_pos_control.set_accel_xy(_loiter_accel_cmss);
// initialise desired acceleration and angles to zero to remain on station
_loiter_predicted_accel.x = 0.0f;
_loiter_predicted_accel.y = 0.0f;
_loiter_desired_accel = _loiter_predicted_accel;
_loiter_predicted_euler_angle.x = 0.0f;
_loiter_predicted_euler_angle.y = 0.0f;
// set target position
_pos_control.set_xy_target(position.x, position.y);
// set vehicle velocity and acceleration to zero
_pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.set_desired_accel_xy(0.0f,0.0f);
// initialise position controller
_pos_control.init_xy_controller();
}
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::init_loiter_target()
{
const Vector3f& curr_pos = _inav.get_position();
const Vector3f& curr_vel = _inav.get_velocity();
// sanity check loiter speed
_loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN);
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(WPNAV_LOITER_VEL_CORRECTION_MAX);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_leash_length_xy(WPNAV_LOITER_POS_CORRECTION_MAX);
// initialise desired acceleration based on the current velocity and the artificial drag
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_loiter_angle_max_cd()*0.01f));
_loiter_predicted_accel.x = pilot_acceleration_max*curr_vel.x/_loiter_speed_cms;
_loiter_predicted_accel.y = pilot_acceleration_max*curr_vel.y/_loiter_speed_cms;
_loiter_desired_accel = _loiter_predicted_accel;
// this should be the current roll and pitch angle.
_loiter_predicted_euler_angle.x = radians(_attitude_control.get_att_target_euler_cd().x*0.01f);
_loiter_predicted_euler_angle.y = radians(_attitude_control.get_att_target_euler_cd().y*0.01f);
// set target position
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
// set vehicle velocity and acceleration to current state
_pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
_pos_control.set_desired_accel_xy(_loiter_desired_accel.x,_loiter_desired_accel.y);
// initialise position controller
_pos_control.init_xy_controller();
}
/// loiter_soften_for_landing - reduce response for landing
void AC_WPNav::loiter_soften_for_landing()
{
const Vector3f& curr_pos = _inav.get_position();
// set target position to current position
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
}
/// set pilot desired acceleration in centi-degrees
// dt should be the time (in seconds) since the last call to this function
void AC_WPNav::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
{
// Convert from centidegrees on public interface to radians
const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
// difference between where we think we should be and where we want to be
Vector2f angle_error(wrap_PI(euler_roll_angle-_loiter_predicted_euler_angle.x), wrap_PI(euler_pitch_angle-_loiter_predicted_euler_angle.y));
// calculate the angular velocity that we would expect given our desired and predicted attitude
_attitude_control.input_shaping_rate_predictor(angle_error, _loiter_predicted_euler_rate, dt);
// update our predicted attitude based on our predicted angular velocity
_loiter_predicted_euler_angle += _loiter_predicted_euler_rate * dt;
// convert our desired attitude to an acceleration vector assuming we are hovering
const float pilot_cos_pitch_target = cosf(euler_pitch_angle);
const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);
// convert our predicted attitude to an acceleration vector assuming we are hovering
const float pilot_predicted_cos_pitch_target = cosf(_loiter_predicted_euler_angle.y);
const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_loiter_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_loiter_predicted_euler_angle.y);
// rotate acceleration vectors input to lat/lon frame
_loiter_desired_accel.x = (pilot_accel_fwd_cms*_ahrs.cos_yaw() - pilot_accel_rgt_cms*_ahrs.sin_yaw());
_loiter_desired_accel.y = (pilot_accel_fwd_cms*_ahrs.sin_yaw() + pilot_accel_rgt_cms*_ahrs.cos_yaw());
_loiter_predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
_loiter_predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());
}
/// get_loiter_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
{
_pos_control.get_stopping_point_xy(stopping_point);
}
/// get_loiter_angle_max - returns the maximum lean angle in loiter mode
float AC_WPNav::get_loiter_angle_max_cd() const
{
if (is_zero(_loiter_angle_max)){
return MIN(_attitude_control.lean_angle_max(), _pos_control.get_lean_angle_max_cd()) * (2.0f/3.0f);
}
return MIN(_loiter_angle_max*100.0f, _pos_control.get_lean_angle_max_cd());
}
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
{
// calculate a loiter speed limit which is the minimum of the value set by the WPNAV_LOITER_SPEED
// parameter and the value set by the EKF to observe optical flow limits
float gnd_speed_limit_cms = MIN(_loiter_speed_cms,ekfGndSpdLimit*100.0f);
gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, WPNAV_LOITER_SPEED_MIN);
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_loiter_angle_max_cd()*0.01f));
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
_pos_control.set_speed_xy(gnd_speed_limit_cms);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_leash_length_xy(WPNAV_LOITER_POS_CORRECTION_MAX);
// get loiters desired velocity from the position controller where it is being stored.
const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity();
Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y);
// update the desired velocity using our predicted acceleration
desired_vel.x += _loiter_predicted_accel.x * nav_dt;
desired_vel.y += _loiter_predicted_accel.y * nav_dt;
Vector2f loiter_accel_brake;
float desired_speed = desired_vel.length();
if (!is_zero(desired_speed)) {
Vector2f desired_vel_norm = desired_vel/desired_speed;
// TODO: consider using a velocity squared relationship like
// pilot_acceleration_max*(desired_speed/gnd_speed_limit_cms)^2;
// the drag characteristic of a multirotor should be examined to generate a curve
// we could add a expo function here to fine tune it
// calculate a drag acceleration based on the desired speed.
float drag_decel = pilot_acceleration_max*desired_speed/gnd_speed_limit_cms;
// calculate a braking acceleration if sticks are at zero
float loiter_brake_accel = 0.0f;
if (_loiter_desired_accel.is_zero()) {
if((AP_HAL::millis()-_brake_timer) > _loiter_brake_delay * 1000.0f){
float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f;
loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller(desired_speed, brake_gain, _loiter_brake_jerk_max_cmsss, nav_dt), 0.0f, _loiter_brake_accel_cmss);
}
} else {
loiter_brake_accel = 0.0f;
_brake_timer = AP_HAL::millis();
}
_loiter_brake_accel += constrain_float(loiter_brake_accel-_loiter_brake_accel, -_loiter_brake_jerk_max_cmsss*nav_dt, _loiter_brake_jerk_max_cmsss*nav_dt);
loiter_accel_brake = desired_vel_norm*_loiter_brake_accel;
// update the desired velocity using the drag and braking accelerations
desired_speed = MAX(desired_speed-(drag_decel+_loiter_brake_accel)*nav_dt,0.0f);
desired_vel = desired_vel_norm*desired_speed;
}
// add braking to the desired acceleration
_loiter_desired_accel -= loiter_accel_brake;
// Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
float horizSpdDem = desired_vel.length();
if (horizSpdDem > gnd_speed_limit_cms) {
desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;
desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
}
// Limit the velocity to prevent fence violations
// TODO: We need to also limit the _loiter_desired_accel
if (_avoid != nullptr) {
_avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _loiter_accel_cmss, desired_vel, nav_dt);
}
// send adjusted feed forward acceleration and velocity back to the Position Controller
_pos_control.set_desired_accel_xy(_loiter_desired_accel.x,_loiter_desired_accel.y);
_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
}
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_loiter_bearing_to_target() const
{
return get_bearing_cd(_inav.get_position(), _pos_control.get_pos_target());
}
// update_loiter - run the loiter controller - gets called at 100hz (APM) or 400hz (PX4)
void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
if (dt >= 0.2f) {
dt = 0.0f;
}
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
_pos_control.set_accel_xy(_loiter_accel_cmss);
calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
_pos_control.update_xy_controller(ekfNavVelGainScaler);
}
/// init_brake_target - initializes stop position from current position and velocity /// init_brake_target - initializes stop position from current position and velocity
void AC_WPNav::init_brake_target(float accel_cmss) void AC_WPNav::init_brake_target(float accel_cmss)

74
libraries/AC_WPNav/AC_WPNav.h
View File

@ -10,19 +10,10 @@
#include <AP_Terrain/AP_Terrain.h> #include <AP_Terrain/AP_Terrain.h>
#include <AC_Avoidance/AC_Avoid.h> // Stop at fence library #include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
// loiter maximum velocities and accelerations // maximum velocities and accelerations
#define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller #define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller
#define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter #define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter
#define WPNAV_LOITER_SPEED 1250.0f // default loiter speed in cm/s
#define WPNAV_LOITER_SPEED_MIN 20.0f // minimum loiter speed in cm/s
#define WPNAV_LOITER_ACCEL_MAX 500.0f // default acceleration in loiter mode
#define WPNAV_LOITER_BRAKE_ACCEL 250.0f // minimum acceleration in loiter mode
#define WPNAV_LOITER_BRAKE_JERK 500.0f // maximum jerk in cm/s/s/s in loiter mode
#define WPNAV_LOITER_BRAKE_START_DELAY 1.0f // delay (in seconds) before loiter braking begins after sticks are released
#define WPNAV_LOITER_VEL_CORRECTION_MAX 200.0f // max speed used to correct position errors in loiter
#define WPNAV_LOITER_POS_CORRECTION_MAX 200.0f // max position error in loiter
#define WPNAV_WP_SPEED 500.0f // default horizontal speed between waypoints in cm/s #define WPNAV_WP_SPEED 500.0f // default horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s #define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_TRACK_SPEED_MIN 50.0f // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination) #define WPNAV_WP_TRACK_SPEED_MIN 50.0f // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination)
@ -38,8 +29,6 @@
#define WPNAV_WP_FAST_OVERSHOOT_MAX 200.0f // 2m overshoot is allowed during fast waypoints to allow for smooth transitions to next waypoint #define WPNAV_WP_FAST_OVERSHOOT_MAX 200.0f // 2m overshoot is allowed during fast waypoints to allow for smooth transitions to next waypoint
#define WPNAV_LOITER_ACTIVE_TIMEOUT_MS 200 // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds)
#define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading #define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading
#define WPNAV_YAW_LEASH_PCT_MIN 0.134f // target point must be at least this distance from the vehicle (expressed as a percentage of the maximum distance it can be from the vehicle - i.e. the leash length) #define WPNAV_YAW_LEASH_PCT_MIN 0.134f // target point must be at least this distance from the vehicle (expressed as a percentage of the maximum distance it can be from the vehicle - i.e. the leash length)
@ -68,49 +57,6 @@ public:
/// provide rangefinder altitude /// provide rangefinder altitude
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
///
/// loiter controller
///
/// init_loiter_target to a position in cm from ekf origin
void init_loiter_target(const Vector3f& position);
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void init_loiter_target();
/// loiter_soften_for_landing - reduce response for landing
void loiter_soften_for_landing();
/// 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();
/// set pilot desired acceleration in centi-degrees
// dt should be the time (in seconds) since the last call to this function
void set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt);
/// get_pilot_desired_acceleration - gets pilot desired
/// acceleration, body frame, [forward,right]
Vector2f get_pilot_desired_acceleration() const { return Vector2f(_loiter_desired_accel.x, _loiter_desired_accel.y); }
/// clear_pilot_desired_acceleration - clear pilot desired acceleration
void clear_pilot_desired_acceleration() { _loiter_desired_accel.x = 0.0f; _loiter_desired_accel.y = 0.0f; }
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_loiter_stopping_point_xy(Vector3f& stopping_point) const;
/// get_loiter_distance_to_target - get horizontal distance to loiter target in cm
float get_loiter_distance_to_target() const { return _pos_control.get_distance_to_target(); }
/// get_loiter_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_loiter_bearing_to_target() const;
/// get_loiter_target - returns loiter target position
const Vector3f& get_loiter_target() const { return _pos_control.get_pos_target(); }
/// get_loiter_angle_max - returns the maximum lean angle in loiter mode
float get_loiter_angle_max_cd() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler);
/// ///
/// brake controller /// brake controller
/// ///
@ -281,10 +227,6 @@ protected:
uint8_t wp_yaw_set : 1; // true if yaw target has been set uint8_t wp_yaw_set : 1; // true if yaw target has been set
} _flags; } _flags;
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit);
/// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed /// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed
void calc_slow_down_distance(float speed_cms, float accel_cmss); void calc_slow_down_distance(float speed_cms, float accel_cmss);
@ -323,12 +265,6 @@ protected:
AC_Avoid *_avoid = nullptr; AC_Avoid *_avoid = nullptr;
// parameters // parameters
AP_Float _loiter_angle_max; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max
AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter
AP_Float _loiter_accel_cmss; // loiter's max acceleration in cm/s/s
AP_Float _loiter_brake_accel_cmss; // loiter's acceleration during braking in cm/s/s
AP_Float _loiter_brake_jerk_max_cmsss;
AP_Float _loiter_brake_delay; // delay (in seconds) before loiter braking begins after sticks are released
AP_Float _wp_speed_cms; // maximum horizontal speed in cm/s during missions AP_Float _wp_speed_cms; // maximum horizontal speed in cm/s during missions
AP_Float _wp_speed_up_cms; // climb speed target in cm/s AP_Float _wp_speed_up_cms; // climb speed target in cm/s
AP_Float _wp_speed_down_cms; // descent speed target in cm/s AP_Float _wp_speed_down_cms; // descent speed target in cm/s
@ -336,14 +272,6 @@ protected:
AP_Float _wp_accel_cms; // horizontal acceleration in cm/s/s during missions AP_Float _wp_accel_cms; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cms; // vertical acceleration in cm/s/s during missions AP_Float _wp_accel_z_cms; // vertical acceleration in cm/s/s during missions
// loiter controller internal variables
Vector2f _loiter_desired_accel; // slewed pilot's desired acceleration in lat/lon frame
Vector2f _loiter_predicted_accel;//
Vector2f _loiter_predicted_euler_angle;//
Vector2f _loiter_predicted_euler_rate; //
float _brake_timer; //
float _loiter_brake_accel; //
// waypoint controller internal variables // waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call uint32_t _wp_last_update; // time of last update_wpnav call
uint8_t _wp_step; // used to decide which portion of wpnav controller to run during this iteration uint8_t _wp_step; // used to decide which portion of wpnav controller to run during this iteration