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

AC_PosControl: add accel filter parameter

Browse Source
This commit is contained in:
Leonard Hall 2015-04-16 19:37:33 +09:30 committed by Randy Mackay
parent 1fcb0f4d53
commit c2a6a0a9e2
2 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -5,7 +5,16 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = {
// 0 was used for HOVER
// 0 was used for HOVER
// @Param: _ACC_XY_FILT
// @DisplayName: XY Acceleration filter cutoff frequency
// @Description: Lower values will slow the response of the navigation controller and reduce twitchiness
// @Units: Hz
// @Range: 0.5 5
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("_ACC_XY_FILT", 1, AC_PosControl, _accel_xy_filt_hz, POSCONTROL_ACCEL_FILTER_HZ),
AP_GROUPEND
};
@ -134,12 +143,12 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
{
// jurk is calculated to reach full acceleration in 500ms.
float jurk_z = _accel_z_cms * 2.0f;
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = _accel_z_cms * POSCONTROL_JERK_RATIO;
float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabs(_vel_desired.z - climb_rate_cms)*jurk_z));
float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabs(_vel_desired.z - climb_rate_cms)*jerk_z));
_accel_last_z_cms += jurk_z * dt;
_accel_last_z_cms += jerk_z * dt;
_accel_last_z_cms = min(accel_z_max, _accel_last_z_cms);
float vel_change_limit = _accel_last_z_cms * dt;
@ -849,8 +858,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
}
_accel_target_jerk_limited += accel_change;
// 5Hz lowpass filter on NE accel
float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler;
// lowpass filter on NE accel
float freq_cut = min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler);
float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f);
_accel_target_filtered.x += alpha * (_accel_target_jerk_limited.x - _accel_target_filtered.x);
_accel_target_filtered.y += alpha * (_accel_target_jerk_limited.y - _accel_target_filtered.y);

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -41,7 +41,8 @@
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // 4hz low-pass filter on velocity error
#define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // 2hz low-pass filter on accel error
#define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // 17m/s/s/s jerk limit on horizontal acceleration
#define POSCONTROL_ACCEL_FILTER_HZ 5.0f // 5hz low-pass filter on acceleration
#define POSCONTROL_ACCEL_FILTER_HZ 2.0f // 5hz low-pass filter on acceleration
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
class AC_PosControl
{
@ -344,6 +345,9 @@ private:
AC_P& _p_pos_xy;
AC_PI_2D& _pi_vel_xy;
// parameters
AP_Float _accel_xy_filt_hz; // XY acceleration filter cutoff frequency
// internal variables
float _dt; // time difference (in seconds) between calls from the main program
float _dt_xy; // time difference (in seconds) between update_xy_controller and update_vel_controller_xyz calls