WPNav: add yaw control for straight line wp nav

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;

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