guided velocity control initializes max speed and accel from WPNAV pa…

…rameters as well

ArduCopter/control_guided.cpp

@@ -107,9 +107,18 @@ void Copter::guided_vel_control_start()
     // set guided_mode to velocity controller
     guided_mode = Guided_Velocity;
 
-    // initialize vertical speeds and leash lengths
-    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
-    pos_control.set_accel_z(g.pilot_accel_z);
+//    // initialize vertical speeds and leash lengths
+//    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+//    pos_control.set_accel_z(g.pilot_accel_z);
+
+    // set horizontal speed and acceleration from wpnav's speed and acceleration
+    pos_control.set_speed_xy(wp_nav.get_speed_xy());
+    pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
+    pos_control.set_jerk_xy_to_default(); // POSCONTROL_JERK_LIMIT_CMSSS = 1700 m/s/s/s
+
+    // set vertical speed and acceleration for wpnav's speed and acceleration
+    pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
+    pos_control.set_accel_z(wp_nav.get_accel_z());
 
     // initialise velocity controller
     pos_control.init_vel_controller_xyz();