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

AC_AttitudeControl: AC_PosControl: Support error input to kinematic shaper

This commit is contained in:
Leonard Hall 2021-12-05 15:24:54 +10:30 committed by Randy Mackay
parent 37c6dc74ae
commit e8a067d99c
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -355,10 +355,10 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float
accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms; accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms;
} }
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
// adjust desired altitude if motors have not hit their limits // adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
// calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos
float vel_max_xy_cms = 0.0f; float vel_max_xy_cms = 0.0f;
@ -536,7 +536,7 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
// check for ekf xy position reset // check for ekf xy position reset
handle_ekf_xy_reset(); handle_ekf_xy_reset();
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
shape_accel_xy(accel, _accel_desired, _jerk_max_xy_cmsss, _dt); shape_accel_xy(accel, _accel_desired, _jerk_max_xy_cmsss, _dt);
} }
@ -547,12 +547,12 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output) void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output)
{ {
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(), shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(),
_accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output); _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output);
update_vel_accel_xy(vel, accel, _dt, Vector2f()); update_vel_accel_xy(vel, accel, _dt, Vector2f(), Vector2f());
} }
/// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
@ -562,12 +562,12 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, boo
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output) void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output)
{ {
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy()); update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
_vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output); _vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output);
update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f()); update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f(), Vector2f(), Vector2f());
} }
/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system /// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
@ -647,7 +647,7 @@ void AC_PosControl::update_xy_controller()
_vehicle_horiz_vel.x = _inav.get_velocity().x; _vehicle_horiz_vel.x = _inav.get_velocity().x;
_vehicle_horiz_vel.y = _inav.get_velocity().y; _vehicle_horiz_vel.y = _inav.get_velocity().y;
} }
Vector2f accel_target = _pid_vel_xy.update_all(Vector2f{_vel_target.x, _vel_target.y}, _vehicle_horiz_vel, Vector2f(_limit_vector.x, _limit_vector.y)); Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), _vehicle_horiz_vel, _limit_vector.xy());
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ekfNavVelGainScaler; accel_target *= ekfNavVelGainScaler;
@ -824,7 +824,7 @@ void AC_PosControl::input_accel_z(float accel)
} }
// adjust desired alt if motors have not hit their limits // adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
shape_accel(accel, _accel_desired.z, _jerk_max_z_cmsss, _dt); shape_accel(accel, _accel_desired.z, _jerk_max_z_cmsss, _dt);
} }
@ -850,14 +850,14 @@ void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool ignore_desce
} }
// adjust desired alt if motors have not hit their limits // adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
shape_vel_accel(vel, accel, shape_vel_accel(vel, accel,
_vel_desired.z, _accel_desired.z, _vel_desired.z, _accel_desired.z,
-constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss, -constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
_jerk_max_z_cmsss, _dt, limit_output); _jerk_max_z_cmsss, _dt, limit_output);
update_vel_accel(vel, accel, _dt, 0); update_vel_accel(vel, accel, _dt, 0.0, 0.0);
} }
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
@ -907,7 +907,7 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, float accel, b
} }
// adjust desired altitude if motors have not hit their limits // adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
shape_pos_vel_accel(pos, vel, accel, shape_pos_vel_accel(pos, vel, accel,
_pos_target.z, _vel_desired.z, _accel_desired.z, _pos_target.z, _vel_desired.z, _accel_desired.z,
@ -916,7 +916,7 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, float accel, b
_jerk_max_z_cmsss, _dt, limit_output); _jerk_max_z_cmsss, _dt, limit_output);
postype_t posp = pos; postype_t posp = pos;
update_pos_vel_accel(posp, vel, accel, _dt, 0); update_pos_vel_accel(posp, vel, accel, _dt, 0.0, 0.0, 0.0);
pos = posp; pos = posp;
} }
@ -933,7 +933,7 @@ void AC_PosControl::update_pos_offset_z(float pos_offset_z)
{ {
postype_t p_offset_z = _pos_offset_z; postype_t p_offset_z = _pos_offset_z;
update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f)); update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error());
_pos_offset_z = p_offset_z; _pos_offset_z = p_offset_z;
// input shape the terrain offset // input shape the terrain offset

4
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
View File

@ -33,7 +33,7 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
} }
// adjust desired alt if motors have not hit their limits // adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z); update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
// prevent altitude target from breeching altitude limits // prevent altitude target from breeching altitude limits
if (is_negative(_alt_min) && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) { if (is_negative(_alt_min) && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) {
@ -45,5 +45,5 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo
-accel_z_cms, accel_z_cms, -accel_z_cms, accel_z_cms,
_jerk_max_xy_cmsss, _dt, limit_output); _jerk_max_xy_cmsss, _dt, limit_output);
update_vel_accel(vel, accel, _dt, _limit_vector.z); update_vel_accel(vel, accel, _dt, 0.0, 0.0);
} }