Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-20 07:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: get_yaw_cd points along track

Browse Source
This commit is contained in:
Randy Mackay 2024-08-28 13:12:34 +09:00
parent 0c66607858
commit 918f634e90
2 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -190,6 +190,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
_flags.reached_destination = true;
_flags.fast_waypoint = false;
_flags.wp_yaw_set = false;
// initialise origin and destination to stopping point
if (stopping_point.is_zero()) {
@ -456,6 +457,16 @@ void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const
stopping_point = stop.tofloat();
}
// get target yaw in centi-degrees along the path to the next waypoint
// this may not point directly at the waypoint especially during cornering. call is_active() before using
float AC_WPNav::get_yaw_cd() const
{
if (_flags.wp_yaw_set) {
return _wp_yaw_cd;
}
return _pos_control.get_yaw_cd();
}
/// advance_wp_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_wp_target_along_track(float dt)
{
@ -535,6 +546,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
s_finished = _spline_this_leg.reached_destination();
}
// update the target yaw if velocity is greater than 5% _vel_max_xy_cms
if (is_positive(target_vel.xy().length_squared())) {
if (target_vel.xy().length() > _wp_desired_speed_xy_cms * 0.05f) {
_wp_yaw_cd = degrees(target_vel.xy().angle()) * 100.0;
_flags.wp_yaw_set = true;
}
}
#if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// use raw targets to calculate maximum avoidance velocity
_simple_avoidance.vel_xy_max_set = false;

9
libraries/AC_WPNav/AC_WPNav.h
View File

@ -209,8 +209,10 @@ public:
float get_pitch() const { return _pos_control.get_pitch_cd(); }
Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); }
// get target yaw in centi-degrees
float get_yaw() const { return _pos_control.get_yaw_cd(); }
// get target yaw in centi-degrees along the path to the next waypoint
// this may not point directly at the waypoint especially during cornering. call is_active() before using
float get_yaw_cd() const;
/// advance_wp_target_along_track - move target location along track from origin to destination
bool advance_wp_target_along_track(float dt);
@ -228,7 +230,7 @@ protected:
struct wpnav_flags {
uint8_t reached_destination : 1; // true if we have reached the destination
uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
uint8_t wp_yaw_set : 1; // true if yaw target has been set
uint8_t wp_yaw_set : 1; // true if _wp_yaw_cd has been set
} _flags;
// helper function to calculate scurve jerk and jerk_time values
@ -283,6 +285,7 @@ protected:
float _offset_vel; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
float _offset_accel; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
bool _paused; // flag for pausing waypoint controller
float _wp_yaw_cd; // yaw along path to next waypoint (may not point directly at waypoint). only valid is _flags.wp_yaw_set and is_active() are true
// terrain following variables
bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin