From 606a0e0d0bb59b27a48d3f7070f9c9799be7185c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 29 Oct 2015 11:47:46 +0900 Subject: [PATCH] Copter: set jerk to default for circle and guided --- ArduCopter/control_circle.cpp | 1 + ArduCopter/control_guided.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 03361349b7..eff4720a9e 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -15,6 +15,7 @@ bool Copter::circle_init(bool ignore_checks) // initialize speeds and accelerations 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(); pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 12d10d17d1..26c44536e1 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -113,6 +113,7 @@ void Copter::guided_posvel_control_start() // set 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(); const Vector3f& curr_pos = inertial_nav.get_position(); const Vector3f& curr_vel = inertial_nav.get_velocity();