From a342b73604ea1233d2de95fae7f129636c172b26 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 27 Apr 2017 13:21:31 +0900 Subject: [PATCH] Copter: guided velocity contoller inits accel and jerk In practice this has no functional impact because the guided_pos_control_start initialises these values (using AC_WPNav) and it is not currently possible for a user to get to the velocity controller without having first used the guided position controller --- ArduCopter/control_guided.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 27774427cc..9992b1be0f 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -107,7 +107,12 @@ void Copter::guided_vel_control_start() // set guided_mode to velocity controller guided_mode = Guided_Velocity; - // initialize vertical speeds and leash lengths + // initialise horizontal speed, acceleration and jerk + 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(); + + // initialize vertical speeds and acceleration pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control->set_accel_z(g.pilot_accel_z);