mirror of https://github.com/ArduPilot/ardupilot
AC_PosControl: add set_jerk_xy
This commit is contained in:
parent
853f8bfaf4
commit
ee0abb1750
|
@ -48,6 +48,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
|
|||
_accel_z_cms(POSCONTROL_ACCEL_Z),
|
||||
_accel_last_z_cms(0.0f),
|
||||
_accel_cms(POSCONTROL_ACCEL_XY),
|
||||
_jerk_cmsss(POSCONTROL_JERK_LIMIT_CMSSS),
|
||||
_leash(POSCONTROL_LEASH_LENGTH_MIN),
|
||||
_leash_down_z(POSCONTROL_LEASH_LENGTH_MIN),
|
||||
_leash_up_z(POSCONTROL_LEASH_LENGTH_MIN),
|
||||
|
@ -920,7 +921,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bo
|
|||
}
|
||||
|
||||
// apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
|
||||
float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS;
|
||||
float max_delta_accel = dt * _jerk_cmsss;
|
||||
|
||||
Vector2f accel_in(_accel_target.x, _accel_target.y);
|
||||
Vector2f accel_change = accel_in-_accel_target_jerk_limited;
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
#define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically
|
||||
// should be 1.5 times larger than POSCONTROL_ACCELERATION.
|
||||
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
|
||||
#define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // default jerk limit on horizontal acceleration (unit: m/s/s/s)
|
||||
|
||||
#define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s
|
||||
#define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s
|
||||
|
@ -39,7 +40,6 @@
|
|||
|
||||
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // low-pass filter on velocity error (unit: hz)
|
||||
#define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz)
|
||||
#define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // jerk limit on horizontal acceleration (unit: m/s/s/s)
|
||||
#define POSCONTROL_ACCEL_FILTER_HZ 2.0f // low-pass filter on acceleration (unit: hz)
|
||||
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
|
||||
|
||||
|
@ -192,6 +192,10 @@ public:
|
|||
void set_speed_xy(float speed_cms);
|
||||
float get_speed_xy() const { return _speed_cms; }
|
||||
|
||||
/// set_jerk_xy - set max horizontal jerk in cm/s/s/s
|
||||
void set_jerk_xy(float jerk_cmsss) { _jerk_cmsss = jerk_cmsss; }
|
||||
void set_jerk_xy_to_default() { _jerk_cmsss = POSCONTROL_JERK_LIMIT_CMSSS; }
|
||||
|
||||
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
|
||||
/// should be called whenever the speed, acceleration or position kP is modified
|
||||
void calc_leash_length_xy();
|
||||
|
@ -377,6 +381,7 @@ private:
|
|||
float _accel_z_cms; // max vertical acceleration in cm/s/s
|
||||
float _accel_last_z_cms; // max vertical acceleration in cm/s/s
|
||||
float _accel_cms; // max horizontal acceleration in cm/s/s
|
||||
float _jerk_cmsss; // max horizontal jerk in cm/s/s/s
|
||||
float _leash; // horizontal leash length in cm. target will never be further than this distance from the vehicle
|
||||
float _leash_down_z; // vertical leash down in cm. target will never be further than this distance below the vehicle
|
||||
float _leash_up_z; // vertical leash up in cm. target will never be further than this distance above the vehicle
|
||||
|
|
Loading…
Reference in New Issue