Copter: fix guided_posvel and guided_vel to update pos_control at same rate as wpnav

ArduCopter/control_guided.pde

@@ -271,8 +271,19 @@ static void guided_vel_control_run()
         }
     }
 
+    // calculate dt
+    float dt = pos_control.time_since_last_xy_update();
+
+    // update at poscontrol update rate
+    if (dt >= pos_control.get_dt_xy()) {
+        // sanity check dt
+        if (dt >= 0.2f) {
+            dt = 0.0f;
+        }
+
         // call velocity controller which includes z axis controller
         pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
+    }
 
     // call attitude controller
     if (auto_yaw_mode == AUTO_YAW_HOLD) {
@@ -305,8 +316,18 @@ static void guided_posvel_control_run()
         posvel_vel_target_cms.zero();
     }
 
+    // calculate dt
+    float dt = pos_control.time_since_last_xy_update();
+
+    // update at poscontrol update rate
+    if (dt >= pos_control.get_dt_xy()) {
+        // sanity check dt
+        if (dt >= 0.2f) {
+            dt = 0.0f;
+        }
+
         // advance position target using velocity target
-    posvel_pos_target_cm += posvel_vel_target_cms * G_Dt;
+        posvel_pos_target_cm += posvel_vel_target_cms * dt;
 
         // send position and velocity targets to position controller
         pos_control.set_pos_target(posvel_pos_target_cm);
@@ -315,6 +336,7 @@ static void guided_posvel_control_run()
         // run position controller
         pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler);
         pos_control.update_z_controller();
+    }
 
     // call attitude controller
     if (auto_yaw_mode == AUTO_YAW_HOLD) {