From 24eb195aa39e494ef4f8b02270a24996b25b552f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 23 Mar 2014 20:33:27 +0900 Subject: [PATCH] WPNav: add yaw control for straight line wp nav --- libraries/AC_WPNav/AC_WPNav.cpp | 9 ++++++--- libraries/AC_WPNav/AC_WPNav.h | 6 +++--- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b0e091da7d..a72247fe95 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -87,7 +87,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro _spline_time(0.0), _spline_vel_scaler(0.0), _spline_slow_down_dist(0.0), - _spline_yaw(0.0) + _yaw(0.0) { AP_Param::setup_object_defaults(this, var_info); } @@ -291,6 +291,9 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto // calculate leash lengths calculate_wp_leash_length(); + // initialise yaw heading + _yaw = get_bearing_cd(_origin, _destination); + // initialise intermediate point to the origin _pos_control.set_pos_target(origin); _track_desired = 0; // target is at beginning of track @@ -595,7 +598,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V } // initialise yaw heading to current heading - _spline_yaw = _ahrs->yaw_sensor; + _yaw = _ahrs->yaw_sensor; // To-Do: handle case where this is a straight segment? // if this is a straight segment, origin velocity is distance vector from origin to destination @@ -701,7 +704,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) _pos_control.set_pos_target(target_pos); // update the yaw - _spline_yaw = RadiansToCentiDegrees(atan2(target_vel.y,target_vel.x)); + _yaw = RadiansToCentiDegrees(atan2(target_vel.y,target_vel.x)); // advance spline time to next step _spline_time += spline_time_scale*0.1f; diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 7808e17edc..74bb5f3f1b 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -146,8 +146,8 @@ public: // straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination // spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination - // get_spline_yaw - returns target yaw in centi-degrees - float get_spline_yaw() { return _spline_yaw; } + // get_yaw - returns target yaw in centi-degrees (used for wp and spline navigation) + float get_yaw() { return _yaw; } /// set_spline_destination waypoint using position vector (distance from home in cm) /// stopped_at_start should be set to true if vehicle is stopped at the origin @@ -261,6 +261,6 @@ protected: float _spline_vel_scaler; // float _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination // To-Do: this should be used for straight segments as well - float _spline_yaw; // heading according to yaw + float _yaw; // heading according to yaw }; #endif // AC_WPNAV_H