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

AC_AttitudeControl:AC_PosControl: vibration failsafe fix

This commit is contained in:
Leonard Hall 2021-06-23 00:09:44 +09:30 committed by Andrew Tridgell
parent 16f7348f96
commit 20158ddf5f
2 changed files with 7 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -897,7 +897,7 @@ void AC_PosControl::update_z_controller()
// Check for vertical controller health
// _speed_down_cms is checked to be non-zero when set
float error_ratio = _vel_error.z / _vel_max_down_cms;
float error_ratio = _pid_vel_z.get_error() / _vel_max_down_cms;
_vel_z_control_ratio += _dt * 0.1f * (0.5 - error_ratio);
_vel_z_control_ratio = constrain_float(_vel_z_control_ratio, 0.0f, 2.0f);
@ -922,7 +922,7 @@ void AC_PosControl::get_stopping_point_z_cm(Vector3f& stopping_point) const
const float curr_pos_z = _inav.get_position().z;
float curr_vel_z = _inav.get_velocity().z;
// if position controller is active add current velocity error to avoid sudden jump in acceleration
// if position controller is active remove the desired velocity component
if (is_active_z()) {
curr_vel_z -= _vel_desired.z;
}
@ -1001,10 +1001,10 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector3f &stopping_point) const
Vector3f curr_vel = _inav.get_velocity();
// add velocity error to current velocity
// if position controller is active remove the desired velocity component
if (is_active_xy()) {
curr_vel.x += _vel_error.x;
curr_vel.y += _vel_error.y;
curr_vel.x -= _vel_desired.x;
curr_vel.y -= _vel_desired.y;
}
// calculate current velocity
@ -1039,12 +1039,11 @@ int32_t AC_PosControl::get_bearing_to_target_cd() const
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
float AC_PosControl::get_throttle_with_vibration_override()
{
_accel_desired.z = 0.0f;
const float thr_per_accelz_cmss = _motors.get_throttle_hover() / (GRAVITY_MSS * 100.0f);
// during vibration compensation use feed forward with manually calculated gain
// ToDo: clear pid_info P, I and D terms for logging
if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_z.get_i()) && is_negative(_vel_error.z)) || (is_negative(_pid_accel_z.get_i()) && is_positive(_vel_error.z)))) {
_pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _vel_error.z * _pid_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_z.get_i()) && is_negative(_pid_vel_z.get_error())) || (is_negative(_pid_accel_z.get_i()) && is_positive(_pid_vel_z.get_error())))) {
_pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _pid_vel_z.get_error() * _pid_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
}
return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f;
}

5
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -249,9 +249,6 @@ public:
// get_vel_target_cms - returns the target velocity in NEU cm/s
const Vector3f& get_vel_target_cms() const { return _vel_target; }
// get_vel_target_cms - returns the target velocity in NEU cm/s
const Vector3f& get_vel_error_cms() const { return _vel_error; }
/// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis
void set_vel_desired_z_cms(float vel_z_cms) {_vel_desired.z = vel_z_cms;}
@ -413,10 +410,8 @@ protected:
Vector3f _pos_target; // target location in NEU cm from home
Vector3f _vel_desired; // desired velocity in NEU cm/s
Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step
Vector3f _vel_error; // error between desired and actual acceleration in cm/s
Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)
Vector3f _accel_target; // acceleration target in NEU cm/s/s
Vector3f _accel_error; // acceleration error in NEU cm/s/s
Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set