aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: update yaw only when track is at least 2m

Browse Source
This commit is contained in:
Randy Mackay 2014-07-06 16:42:16 +09:00
parent 81355d1adf
commit 2b64c511ed
2 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -380,7 +380,12 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
calculate_wp_leash_length(); calculate_wp_leash_length();
// initialise yaw heading // initialise yaw heading
_yaw = get_bearing_cd(_origin, _destination); if (_track_length >= WPNAV_YAW_DIST_MIN) {
_yaw = get_bearing_cd(_origin, _destination);
} else {
// set target yaw to current heading. Alternatively we could pull this from the attitude controller if we had access to it
_yaw = _ahrs.yaw_sensor;
}
// initialise intermediate point to the origin // initialise intermediate point to the origin
_pos_control.set_pos_target(origin); _pos_control.set_pos_target(origin);

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -42,6 +42,8 @@
#define WPNAV_LOITER_ACTIVE_TIMEOUT_MS 200 // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds) #define WPNAV_LOITER_ACTIVE_TIMEOUT_MS 200 // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds)
#define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading
class AC_WPNav class AC_WPNav
{ {
public: public: