Copter: remove jerk when entering RTL or AUTO

Loiter target position was not being initialised properly.
Add a few more comments

ArduCopter/ArduCopter.pde

@@ -3,8 +3,9 @@
 #define THISFIRMWARE "ArduCopter V3.0.0-rc1"
 /*
  *  ArduCopter Version 3.0
- *  Lead author:	Jason Short
- *  Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
+ *  Creator:        Jason Short
+ *  Lead Developer: Randy Mackay
+ *  Based on code and ideas from the Arducopter team: Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
  *  Thanks to:	Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini
  *
  *  This firmware is free software; you can redistribute it and/or
@@ -26,7 +27,7 @@
  *  HappyKillmore		:Mavlink GCS
  *  Hein Hollander      :Octo Support
  *  Igor van Airde      :Control Law optimization
- *  Leonard Hall 		:Flight Dynamics, INAV throttle
+ *  Leonard Hall 		:Flight Dynamics, Throttle, Loiter and Navigation Controllers
  *  Jonathan Challinger :Inertial Navigation
  *  Jean-Louis Naudin   :Auto Landing
  *  Max Levine			:Tri Support, Graphics
@@ -35,7 +36,6 @@
  *  Jani Hiriven		:Testing feedback
  *  John Arne Birkeland	:PPM Encoder
  *  Jose Julio			:Stabilization Control laws
- *  Randy Mackay		:General development and release
  *  Marco Robustini		:Lead tester
  *  Michael Oborne		:Mission Planner GCS
  *  Mike Smith			:Libraries, Coding support
@@ -525,12 +525,12 @@ static uint8_t rtl_state;
 // by the DCM through a few simple equations. They are used throughout the code where cos and sin
 // would normally be used.
 // The cos values are defaulted to 1 to get a decent initial value for a level state
-static float cos_roll_x         = 1;
-static float cos_pitch_x        = 1;
-static float cos_yaw            = 1;
-static float sin_yaw            = 1;
-static float sin_roll           = 1;
-static float sin_pitch          = 1;
+static float cos_roll_x         = 1.0;
+static float cos_pitch_x        = 1.0;
+static float cos_yaw            = 1.0;
+static float sin_yaw;
+static float sin_roll;
+static float sin_pitch;
 
 ////////////////////////////////////////////////////////////////////////////////
 // SIMPLE Mode

libraries/AC_WPNav/AC_WPNav.cpp

@@ -56,7 +56,22 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
     _pid_pos_lon(pid_pos_lon),
     _pid_rate_lat(pid_rate_lat),
     _pid_rate_lon(pid_rate_lon),
-    _lean_angle_max(MAX_LEAN_ANGLE)
+    _loiter_last_update(0),
+    _wpnav_last_update(0),
+    _cos_yaw(1.0),
+    _sin_yaw(0.0),
+    _cos_pitch(1.0),
+    _desired_roll(0),
+    _desired_pitch(0),
+    _target(0,0,0),
+    _pilot_vel_forward_cms(0),
+    _pilot_vel_right_cms(0),
+    _target_vel(0,0,0),
+    _vel_last(0,0,0),
+    _lean_angle_max(MAX_LEAN_ANGLE),
+    dist_error(0,0),
+    desired_vel(0,0),
+    desired_accel(0,0)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -220,18 +235,23 @@ void AC_WPNav::set_destination(const Vector3f& destination)
 /// set_origin_and_destination - set origin and destination using lat/lon coordinates
 void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
 {
+    // store origin and destination locations
     _origin = origin;
     _destination = destination;
     Vector3f pos_delta = _destination - _origin;
 
-    bool climb = pos_delta.z >= 0;  // climb vs descending lead to different leash lengths because speed_up_cms and speed_down_cms can be different
+    // calculate leash lengths 
+    bool climb = pos_delta.z >= 0;  // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
     calculate_leash_length(climb);  // update leash lengths and _vert_track_scale
 
+    // scale up z-axis position delta (i.e. distance) to make later leash length calculations simpler
     pos_delta.z = pos_delta.z * _vert_track_scale;
     _track_length = pos_delta.length();
     _pos_delta_unit = pos_delta/_track_length;
 
+    // initialise intermediate point to the origin
     _track_desired = 0;
+    _target = origin;
     _reached_destination = false;
 }