From 945bdee4525464d8fb4a95bd9ade4f0a1d92c1f2 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 19 Oct 2015 17:05:10 -0700 Subject: [PATCH] Copter: PILOT_VELZ_MAX and PILOT_ACCEL_Z take effect immediately --- ArduCopter/control_althold.cpp | 4 ++++ ArduCopter/control_autotune.cpp | 4 ++++ ArduCopter/control_circle.cpp | 6 ++++++ ArduCopter/control_loiter.cpp | 6 +++++- ArduCopter/control_poshold.cpp | 4 ++++ ArduCopter/control_sport.cpp | 6 +++++- 6 files changed, 28 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index fb97ff5d1c..48c68b11a5 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -30,6 +30,10 @@ void Copter::althold_run() AltHoldModeState althold_state; float takeoff_climb_rate = 0.0f; + // 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); + // apply SIMPLE mode transform to pilot inputs update_simple_mode(); diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index ca9961d0a7..2f8f4757e8 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -262,6 +262,10 @@ void Copter::autotune_run() float target_yaw_rate; int16_t target_climb_rate; + // 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); + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // this should not actually be possible because of the autotune_init() checks if (!ap.auto_armed || !motors.get_interlock()) { diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index eff4720a9e..6481fe5e00 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -35,6 +35,12 @@ void Copter::circle_run() float target_yaw_rate = 0; float target_climb_rate = 0; + // 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_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { // To-Do: add some initialisation of position controllers diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 0b2bb6bd46..5ed36ef7d1 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -14,7 +14,7 @@ bool Copter::loiter_init(bool ignore_checks) // set target to current position wp_nav.init_loiter_target(); - // initialize vertical speed and accelerationj + // initialize vertical speed 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); @@ -37,6 +37,10 @@ void Copter::loiter_run() float target_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f; + // initialize vertical speed 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); + // process pilot inputs unless we are in radio failsafe if (!failsafe.radio) { // apply SIMPLE mode transform to pilot inputs diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 75f287355f..83d469cb10 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -136,6 +136,10 @@ void Copter::poshold_run() float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions const Vector3f& vel = inertial_nav.get_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); + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 76c759638f..7d96f60118 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -9,7 +9,7 @@ // sport_init - initialise sport controller bool Copter::sport_init(bool ignore_checks) { - // initialize vertical speed and accelerationj + // initialize vertical speed 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); @@ -28,6 +28,10 @@ void Copter::sport_run() float target_climb_rate = 0; float takeoff_climb_rate = 0.0f; + // initialize vertical speed 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); + // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || ap.throttle_zero) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);