Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: add set_jerk_xy

This commit is contained in:
Randy Mackay 2015-10-29 11:46:34 +09:00
parent 853f8bfaf4
commit ee0abb1750
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -48,6 +48,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_accel_z_cms(POSCONTROL_ACCEL_Z), _accel_z_cms(POSCONTROL_ACCEL_Z),
_accel_last_z_cms(0.0f), _accel_last_z_cms(0.0f),
_accel_cms(POSCONTROL_ACCEL_XY), _accel_cms(POSCONTROL_ACCEL_XY),
_jerk_cmsss(POSCONTROL_JERK_LIMIT_CMSSS),
_leash(POSCONTROL_LEASH_LENGTH_MIN), _leash(POSCONTROL_LEASH_LENGTH_MIN),
_leash_down_z(POSCONTROL_LEASH_LENGTH_MIN), _leash_down_z(POSCONTROL_LEASH_LENGTH_MIN),
_leash_up_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 // 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_in(_accel_target.x, _accel_target.y);
Vector2f accel_change = accel_in-_accel_target_jerk_limited; Vector2f accel_change = accel_in-_accel_target_jerk_limited;

7
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -22,6 +22,7 @@
#define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically #define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically
// should be 1.5 times larger than POSCONTROL_ACCELERATION. // 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 // 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 500.0f // default horizontal speed in cm/s
#define POSCONTROL_SPEED_DOWN -150.0f // default descent rate 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_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_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_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 #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); void set_speed_xy(float speed_cms);
float get_speed_xy() const { return _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 /// 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 /// should be called whenever the speed, acceleration or position kP is modified
void calc_leash_length_xy(); void calc_leash_length_xy();
@ -377,6 +381,7 @@ private:
float _accel_z_cms; // max vertical acceleration in cm/s/s 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_last_z_cms; // max vertical acceleration in cm/s/s
float _accel_cms; // max horizontal 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; // 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_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 float _leash_up_z; // vertical leash up in cm. target will never be further than this distance above the vehicle