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

AC_PosControl: remove accel error filter and set throttle output filter instead

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-15 21:56:48 -07:00 committed by Randy Mackay
parent b7f7624aac
commit 3e0dab7b2d
2 changed files with 4 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -74,7 +74,6 @@ void AC_PosControl::set_dt(float delta_sec)
// update rate z-axis velocity error and accel error filters
_vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ);
_accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ);
}
/// set_dt_xy - sets time delta in seconds for horizontal controller (i.e. 50hz = 0.02)
@ -354,11 +353,10 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
if (_flags.reset_accel_to_throttle) {
// Reset Filter
_accel_error.z = 0;
_accel_error_filter.reset(0);
_flags.reset_accel_to_throttle = false;
} else {
// calculate accel error and Filter with fc = 2 Hz
_accel_error.z = _accel_error_filter.apply(accel_target_z - z_accel_meas);
// calculate accel error
_accel_error.z = accel_target_z - z_accel_meas;
}
// set input to PID
@ -382,7 +380,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
float thr_out = p+i+d+_throttle_hover;
// send throttle to attitude controller with angle boost
_attitude_control.set_throttle_out(thr_out, true);
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
}
///

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -39,7 +39,7 @@
#define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 0.2 seconds
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // 4hz low-pass filter on velocity error
#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0f // 2hz low-pass filter on accel 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
@ -370,7 +370,6 @@ private:
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence
float _distance_to_target; // distance to position target - for reporting only
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
Vector2f _accel_target_jerk_limited; // acceleration target jerk limited to 100deg/s/s
Vector2f _accel_target_filtered; // acceleration target filtered with 5hz low pass filter