AC_AttitudeControl: AC_PosControl: calculate cross track

This commit is contained in:
Leonard Hall 2021-07-21 10:07:00 +09:30 committed by Andrew Tridgell
parent b916f7742e
commit 3d6dd8cff8
2 changed files with 19 additions and 0 deletions

View File

@ -1175,6 +1175,22 @@ void AC_PosControl::write_log()
}
}
/// crosstrack_error - returns horizontal error to the closest point to the current track
float AC_PosControl::crosstrack_error() const
{
const Vector3f& curr_pos = _inav.get_position();
const Vector2f pos_error = curr_pos.xy() - (_pos_target.xy()).tofloat();
if (is_zero(_vel_desired.xy().length_squared())) {
// crosstrack is the horizontal distance to target when stationary
return pos_error.length();
} else {
// crosstrack is the horizontal distance to the closest point to the current track
const Vector2f vel_unit = _vel_desired.xy().normalized();
const float dot_error = pos_error * vel_unit;
return safe_sqrt(pos_error.length_squared() - sq(dot_error));
}
}
///
/// private methods
///

View File

@ -362,6 +362,9 @@ public:
/// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
float get_vel_z_control_ratio() const { return constrain_float(_vel_z_control_ratio, 0.0f, 1.0f); }
/// crosstrack_error - returns horizontal error to the closest point to the current track
float crosstrack_error() const;
/// standby_xyz_reset - resets I terms and removes position error
/// This function will let Loiter and Alt Hold continue to operate
/// in the event that the flight controller is in control of the