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

AC_PosControl: 2hz filter on accel error

Browse Source 
Replaced hard-coded filter with LowPassFilter class allowing the
filter's to be 2hz on both APM and Pixhawk
This commit is contained in:
Randy Mackay 2014-09-21 17:52:14 +09:00
parent 9693ee0417
commit e047093013
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -84,6 +84,9 @@ void AC_PosControl::set_dt(float delta_sec)
// update rate controller's d filter
_pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt);
// update rate z-axis velocity error and accel error filters
_accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ);
}
/// set_speed_z - sets maximum climb and descent rates
@ -365,11 +368,11 @@ 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
// To-Do: replace constant below with one that is adjusted for update rate
_accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z);
_accel_error.z = _accel_error_filter.apply(constrain_float(accel_target_z - z_accel_meas, -32000, 32000));
}
// separately calculate p, i, d values for logging

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -40,6 +40,8 @@
#define POSCONTROL_VEL_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error
class AC_PosControl
{
public:
@ -367,6 +369,7 @@ private:
float _distance_to_target; // distance to position target - for reporting only
uint8_t _xy_step; // used to decide which portion of horizontal position controller to run during this iteration
float _dt_xy; // time difference in seconds between horizontal position updates
LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
// velocity controller internal variables
uint8_t _vel_xyz_step; // used to decide which portion of velocity controller to run during this iteration