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 to use .xy()

This commit is contained in:
Leonard Hall 2021-08-25 14:27:21 +09:30 committed by Randy Mackay
parent 8223d664a7
commit ff58054d1b
1 changed files with 26 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -486,8 +486,7 @@ void AC_PosControl::relax_velocity_controller_xy()
// decay resultant acceleration and therefore current attitude target to zero
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
_accel_target.x *= decay;
_accel_target.y *= decay;
_accel_target.xy() *= decay;
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
}
@ -507,11 +506,8 @@ void AC_PosControl::init_xy()
_pos_target.y = curr_pos.y;
const Vector3f &curr_vel = _inav.get_velocity();
_vel_desired.x = curr_vel.x;
_vel_desired.y = curr_vel.y;
_vel_target.x = curr_vel.x;
_vel_target.y = curr_vel.y;
_vel_desired.xy() = curr_vel.xy();
_vel_target.xy() = curr_vel.xy();
const Vector3f &curr_accel = _ahrs.get_accel_ef_blended() * 100.0f;
_accel_desired.xy() = curr_accel.xy();
@ -578,23 +574,19 @@ void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const V
void AC_PosControl::stop_pos_xy_stabilisation()
{
const Vector3f& curr_pos = _inav.get_position();
_pos_target.x = curr_pos.x;
_pos_target.y = curr_pos.y;
_pos_target.xy() = curr_pos.xy().topostype();
}
/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
void AC_PosControl::stop_vel_xy_stabilisation()
{
const Vector3f curr_pos = _inav.get_position();
_pos_target.x = curr_pos.x;
_pos_target.y = curr_pos.y;
_pos_target.xy() = curr_pos.xy().topostype();
const Vector3f &curr_vel = _inav.get_velocity();
_vel_desired.x = curr_vel.x;
_vel_desired.y = curr_vel.y;
_vel_desired.xy() = curr_vel.xy();
// with zero position error _vel_target = _vel_desired
_vel_target.x = curr_vel.x;
_vel_target.y = curr_vel.y;
_vel_target.xy() = curr_vel.xy();
// initialise I terms from lean angles
_pid_vel_xy.reset_filter();
@ -636,10 +628,8 @@ void AC_PosControl::update_xy_controller()
// add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
vel_target *= ahrsControlScaleXY;
_vel_target.x = vel_target.x;
_vel_target.y = vel_target.y;
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
_vel_target.xy() = vel_target;
_vel_target.xy() += _vel_desired.xy();
// Velocity Controller
@ -648,31 +638,26 @@ void AC_PosControl::update_xy_controller()
if (_flags.vehicle_horiz_vel_override) {
_flags.vehicle_horiz_vel_override = false;
} else {
_vehicle_horiz_vel.x = _inav.get_velocity().x;
_vehicle_horiz_vel.y = _inav.get_velocity().y;
_vehicle_horiz_vel = _inav.get_velocity().xy();
}
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));
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ahrsControlScaleXY;
// pass the correction acceleration to the target acceleration output
_accel_target.x = accel_target.x;
_accel_target.y = accel_target.y;
_accel_target.xy() = accel_target;
// Add feed forward into the target acceleration output
_accel_target.x += _accel_desired.x;
_accel_target.y += _accel_desired.y;
_accel_target.xy() += _accel_desired.xy();
// Acceleration Controller
// limit acceleration using maximum lean angles
_limit_vector.x = 0.0f;
_limit_vector.y = 0.0f;
_limit_vector.xy().zero();
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
if (_accel_target.limit_length_xy(accel_max)) {
_limit_vector.x = _accel_target.x;
_limit_vector.y = _accel_target.y;
_limit_vector.xy() = _accel_target.xy();
}
// update angle targets that will be passed to stabilize controller
@ -1043,12 +1028,9 @@ void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel,
/// set position, velocity and acceleration targets
void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel)
{
_pos_target.x = pos.x;
_pos_target.y = pos.y;
_vel_desired.x = vel.x;
_vel_desired.y = vel.y;
_accel_desired.x = accel.x;
_accel_desired.y = accel.y;
_pos_target.xy() = pos;
_vel_desired.xy() = vel;
_accel_desired.xy() = accel;
}
// get_lean_angles_to_accel - convert roll, pitch lean target angles to lat/lon frame accelerations in cm/s/s
@ -1101,8 +1083,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
// convert the stopping distance into a stopping point using velocity vector
const float t = stopping_dist / vel_total;
stopping_point.x += t * curr_vel.x;
stopping_point.y += t * curr_vel.y;
stopping_point += (curr_vel * t).topostype();
}
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
@ -1231,23 +1212,21 @@ bool AC_PosControl::calculate_yaw_and_rate_yaw()
{
// Calculate the turn rate
float turn_rate = 0.0f;
const Vector2f vel_desired_xy(_vel_desired.x, _vel_desired.y);
const Vector2f accel_desired_xy(_accel_desired.x, _accel_desired.y);
const float vel_desired_xy_len = vel_desired_xy.length();
const float vel_desired_xy_len = _vel_desired.xy().length();
if (is_positive(vel_desired_xy_len)) {
const float accel_forward = (accel_desired_xy.x * vel_desired_xy.x + accel_desired_xy.y * vel_desired_xy.y)/vel_desired_xy_len;
const Vector2f accel_turn = accel_desired_xy - vel_desired_xy * accel_forward / vel_desired_xy_len;
const float accel_forward = (_accel_desired.x * _vel_desired.x + _accel_desired.y * _vel_desired.y) / vel_desired_xy_len;
const Vector2f accel_turn = _accel_desired.xy() - _vel_desired.xy() * accel_forward / vel_desired_xy_len;
const float accel_turn_xy_len = accel_turn.length();
turn_rate = accel_turn_xy_len / vel_desired_xy_len;
if ((accel_turn.y * vel_desired_xy.x - accel_turn.x * vel_desired_xy.y) < 0.0) {
if ((accel_turn.y * _vel_desired.x - accel_turn.x * _vel_desired.y) < 0.0) {
turn_rate = -turn_rate;
}
}
// update the target yaw if velocity is greater than 5% _vel_max_xy_cms
if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) {
_yaw_target = degrees(vel_desired_xy.angle()) * 100.0f;
_yaw_rate_target = turn_rate*degrees(100.0f);
_yaw_target = degrees(_vel_desired.xy().angle()) * 100.0f;
_yaw_rate_target = turn_rate * degrees(100.0f);
return true;
}
return false;
@ -1269,12 +1248,10 @@ void AC_PosControl::handle_ekf_xy_reset()
if (reset_ms != _ekf_xy_reset_ms) {
const Vector3f& curr_pos = _inav.get_position();
_pos_target.x = curr_pos.x + _p_pos_xy.get_error().x;
_pos_target.y = curr_pos.y + _p_pos_xy.get_error().y;
_pos_target.xy() = (curr_pos.xy() + _p_pos_xy.get_error()).topostype();
const Vector3f& curr_vel = _inav.get_velocity();
_vel_target.x = curr_vel.x + _pid_vel_xy.get_error().x;
_vel_target.y = curr_vel.y + _pid_vel_xy.get_error().y;
_vel_target.xy() = curr_vel.xy() + _pid_vel_xy.get_error();
_ekf_xy_reset_ms = reset_ms;
}