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

AC_AttitudeControl: AC_PosControl: Clean up init functions and limit initial xy accelerations based on max lean angle

This commit is contained in:
Leonard Hall 2022-01-01 21:48:00 +10:30 committed by Andrew Tridgell
parent 835e0be245
commit 894b491faa
2 changed files with 27 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -441,24 +441,12 @@ void AC_PosControl::set_correction_speed_accel_xy(float speed_cms, float accel_c
_p_pos_xy.set_limits(speed_cms, accel_cmss, 0.0f);
}
/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void AC_PosControl::init_xy_controller()
{
init_xy();
// set resultant acceleration to current attitude target
Vector3f accel_target;
lean_angles_to_accel_xy(accel_target.x, accel_target.y);
_pid_vel_xy.set_integrator(accel_target - _accel_desired);
}
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void AC_PosControl::init_xy_controller_stopping_point()
{
init_xy();
init_xy_controller();
get_stopping_point_xy_cm(_pos_target.xy());
_vel_desired.xy().zero();
@ -471,7 +459,7 @@ void AC_PosControl::init_xy_controller_stopping_point()
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void AC_PosControl::relax_velocity_controller_xy()
{
init_xy();
init_xy_controller();
// decay resultant acceleration and therefore current attitude target to zero
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
@ -480,9 +468,10 @@ void AC_PosControl::relax_velocity_controller_xy()
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
}
/// init_xy - initialise the position controller to the current position, velocity and acceleration.
/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function is private and contains all the shared xy axis initialisation functions
void AC_PosControl::init_xy()
void AC_PosControl::init_xy_controller()
{
// set roll, pitch lean angle targets to current attitude
const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd();
@ -503,6 +492,11 @@ void AC_PosControl::init_xy()
lean_angles_to_accel_xy(_accel_target.x, _accel_target.y);
// limit acceleration using maximum lean angles
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
_accel_target.xy().limit_length(accel_max);
// initialise I terms from lean angles
_pid_vel_xy.reset_filter();
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
@ -698,17 +692,6 @@ void AC_PosControl::set_correction_speed_accel_z(float speed_down, float speed_u
_p_pos_z.set_limits(-fabsf(speed_down), speed_up, accel_cmss, 0.0f);
}
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void AC_PosControl::init_z_controller()
{
// Initialise the position controller to the current position, velocity and acceleration.
init_z();
// Set accel PID I term based on the current throttle
_pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
}
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function does not allow any negative velocity or acceleration
@ -742,7 +725,7 @@ void AC_PosControl::init_z_controller_stopping_point()
void AC_PosControl::relax_z_controller(float throttle_setting)
{
// Initialise the position controller to the current position, velocity and acceleration.
init_z();
init_z_controller();
// Set accel PID I term based on the requested throttle
float throttle = _attitude_control.get_throttle_in();
@ -750,9 +733,10 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
_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_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function is private and contains all the shared z axis initialisation functions
void AC_PosControl::init_z()
void AC_PosControl::init_z_controller()
{
_pos_target.z = _inav.get_position_z_up_cm();
@ -777,6 +761,9 @@ void AC_PosControl::init_z()
_vel_offset_z = 0.0;
_accel_offset_z = 0.0;
// Set accel PID I term based on the current throttle
_pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
// initialise ekf z reset handler
init_ekf_z_reset();

26
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -87,10 +87,6 @@ public:
void set_pos_error_max_xy_cm(float error_max) { _p_pos_xy.set_error_max(error_max); }
float get_pos_error_max_xy_cm() { return _p_pos_xy.get_error_max(); }
/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void init_xy_controller();
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
@ -100,6 +96,11 @@ public:
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void relax_velocity_controller_xy();
// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function is private and contains all the shared xy axis initialisation functions
void init_xy_controller();
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
@ -166,10 +167,6 @@ public:
/// get_max_speed_down_cms - accessors for current maximum down speed in cm/s. Will be a negative number
float get_max_speed_down_cms() const { return _vel_max_down_cms; }
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void init_z_controller();
/// init_z_controller_no_descent - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function does not allow any negative velocity or acceleration
@ -184,6 +181,11 @@ public:
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void relax_z_controller(float throttle_setting);
// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function is private and contains all the shared z axis initialisation functions
void init_z_controller();
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
@ -390,14 +392,6 @@ protected:
uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
} _flags;
/// init_xy - initialise the position controller to the current position, velocity and acceleration.
/// This function is private and contains all the shared xy axis initialisation functions
void init_xy();
/// init_z - initialise the position controller to the current position, velocity and acceleration.
/// This function is private and contains all the shared z axis initialisation functions
void init_z();
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
float get_throttle_with_vibration_override();