AC_WPNav: add reference to AC_PosControl

Also remove requirement to pass in inertial nav position and velocity to
init_loiter method

libraries/AC_WPNav/AC_WPNav.cpp

@@ -71,6 +71,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
 AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
     _inav(inav),
     _ahrs(ahrs),
+    _pos_control(pos_control),
     _pid_pos_lat(pid_pos_lat),
     _pid_pos_lon(pid_pos_lon),
     _pid_rate_lat(pid_rate_lat),
@@ -108,6 +109,15 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
 /// simple loiter controller
 ///
 
+/// init_loiter_target - sets initial loiter target based on current position and velocity
+void AC_WPNav::init_loiter_target()
+{
+    _pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity());
+    // initialise pilot input
+    _pilot_vel_forward_cms = 0;
+    _pilot_vel_right_cms = 0;
+}
+
 /// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
 void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const
 {
@@ -240,16 +250,10 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
     }
 }
 
-/// get_distance_to_target - get horizontal distance to loiter target in cm
-float AC_WPNav::get_distance_to_target() const
-{
-    return _distance_to_target;
-}
-
 /// get_bearing_to_target - get bearing to loiter target in centi-degrees
 int32_t AC_WPNav::get_bearing_to_target() const
 {
-    return get_bearing_cd(_inav->get_position(), _target);
+    return get_bearing_cd(_inav->get_position(), _pos_control.get_post_target());
 }
 
 /// update_loiter - run the loiter controller - should be called at 10hz

libraries/AC_WPNav/AC_WPNav.h

@@ -9,6 +9,7 @@
 #include <AC_PID.h>             // PID library
 #include <APM_PI.h>             // PID library
 #include <AP_InertialNav.h>     // Inertial Navigation library
+#include <AC_PosControl.h>      // Position control library
 
 // loiter maximum velocities and accelerations
 #define WPNAV_ACCELERATION              100.0f      // defines the default velocity vs distant curve.  maximum acceleration in cm/s/s that position controller asks for from acceleration controller
@@ -40,7 +41,7 @@ class AC_WPNav
 public:
 
     /// Constructor
-    AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
+    AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
 
     ///
     /// simple loiter controller
@@ -52,8 +53,8 @@ public:
     /// set_loiter_target in cm from home
     void set_loiter_target(const Vector3f& position);
 
-    /// init_loiter_target - set initial loiter target based on current position and velocity
-    void init_loiter_target(const Vector3f& position, const Vector3f& velocity);
+    /// init_loiter_target - sets initial loiter target based on current position and velocity
+    void init_loiter_target() { _pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity()); }
 
     /// move_loiter_target - move destination using pilot input
     void move_loiter_target(float control_roll, float control_pitch, float dt);
@@ -182,6 +183,7 @@ protected:
     // references to inertial nav and ahrs libraries
     const AP_InertialNav* const _inav;
     const AP_AHRS*        const _ahrs;
+    AC_PosControl&              _pos_control;
 
     // pointers to pid controllers
     APM_PI*		const _pid_pos_lat;
@@ -224,7 +226,6 @@ protected:
     Vector3f    _pos_delta_unit;        // each axis's percentage of the total track from origin to destination
     float       _track_length;          // distance in cm between origin and destination
     float       _track_desired;         // our desired distance along the track in cm
-    float       _distance_to_target;    // distance to loiter target
     float       _wp_leash_xy;           // horizontal leash length in cm
     float       _wp_leash_z;            // horizontal leash length in cm
     float       _limited_speed_xy_cms;  // horizontal speed in cm/s used to advance the intermediate target towards the destination.  used to limit extreme acceleration after passing a waypoint