Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: 4hz filter on z-axis velocity error

Browse Source
This commit is contained in:
Randy Mackay 2014-09-21 17:53:10 +09:00
parent e047093013
commit 5e197407b9
2 changed files with 6 additions and 1 deletions
libraries/AC_AttitudeControl
AC_PosControl.cppAC_PosControl.h

5
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -86,6 +86,7 @@ void AC_PosControl::set_dt(float delta_sec)
_pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt); _pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt);
// update rate z-axis velocity error and accel error filters // 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); _accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ);
} }
@ -337,10 +338,12 @@ void AC_PosControl::rate_to_accel_z()
if (_flags.reset_rate_to_accel_z) { if (_flags.reset_rate_to_accel_z) {
// Reset Filter // Reset Filter
_vel_error.z = 0; _vel_error.z = 0;
_vel_error_filter.reset(0);
desired_accel = 0; desired_accel = 0;
_flags.reset_rate_to_accel_z = false; _flags.reset_rate_to_accel_z = false;
} else { } else {
_vel_error.z = (_vel_target.z - curr_vel.z); // calculate rate error and filter with cut off frequency of 2 Hz
_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z);
} }
// calculate p // calculate p

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -40,6 +40,7 @@
#define POSCONTROL_VEL_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple) #define POSCONTROL_VEL_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0 // 4hz low-pass filter on velocity error
#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error #define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error
class AC_PosControl class AC_PosControl
@ -369,6 +370,7 @@ private:
float _distance_to_target; // distance to position target - for reporting only 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 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 float _dt_xy; // time difference in seconds between horizontal position updates
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
// velocity controller internal variables // velocity controller internal variables