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

AC_AttitudeControl: AC_PosControl: use relax_integrator

This commit is contained in:
Leonard Hall 2022-01-25 10:13:19 +10:30 committed by Randy Mackay
parent 57c0c14fac
commit 3a1f2847ec
2 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -763,10 +763,9 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
// Initialise the position controller to the current position, velocity and acceleration. // Initialise the position controller to the current position, velocity and acceleration.
init_z(); init_z();
// Set accel PID I term based on the requested throttle // init_z_controller has set the accel PID I term to generate the current throttle set point
float throttle = _attitude_control.get_throttle_in(); // Use relax_integrator to decay the throttle set point to throttle_setting
throttle_setting = throttle + (throttle_setting - throttle) * (_dt / (_dt + POSCONTROL_RELAX_TC)); _pid_accel_z.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f, POSCONTROL_RELAX_TC);
_pid_accel_z.set_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f);
} }
/// init_z - initialise the position controller to the current position, velocity and acceleration. /// init_z - initialise the position controller to the current position, velocity and acceleration.
@ -797,6 +796,13 @@ void AC_PosControl::init_z()
_vel_offset_z = 0.0; _vel_offset_z = 0.0;
_accel_offset_z = 0.0; _accel_offset_z = 0.0;
// Set accel PID I term based on the current throttle
// Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss
// Remove the expected FF term due to non-zero _accel_target.z
_pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f
- _pid_accel_z.kP() * (_accel_target.z - get_z_accel_cmss())
- _pid_accel_z.ff() * _accel_target.z);
// initialise ekf z reset handler // initialise ekf z reset handler
init_ekf_z_reset(); init_ekf_z_reset();

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -34,7 +34,7 @@
#define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range #define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
#define POSCONTROL_RELAX_TC 0.16f // This is used to decay the relevant variable to 5% in half a second. #define POSCONTROL_RELAX_TC 0.16f // This is used to decay the I term to 5% in half a second.
class AC_PosControl class AC_PosControl
{ {