From 362a43c1265a155e9c3f8c34f64988ec9909a7da Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 5 Jun 2015 15:11:30 +0900 Subject: [PATCH] Copter: explicitly set alt_target_from_climb_rate parameter --- ArduCopter/control_althold.cpp | 2 +- ArduCopter/control_autotune.cpp | 2 +- ArduCopter/control_brake.cpp | 7 ++----- ArduCopter/control_circle.cpp | 2 +- ArduCopter/control_loiter.cpp | 2 +- ArduCopter/control_poshold.cpp | 2 +- ArduCopter/control_sport.cpp | 2 +- 7 files changed, 8 insertions(+), 11 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 7b438bac55..61ace7ff21 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -80,7 +80,7 @@ void Copter::althold_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 34c7ce74d5..940537ae03 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -325,7 +325,7 @@ void Copter::autotune_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); } } diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index c61b5ff07b..234d1344e5 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -34,9 +34,6 @@ bool Copter::brake_init(bool ignore_checks) // should be called at 100hz or more void Copter::brake_run() { - float target_yaw_rate = 0; - float target_climb_rate = 0; - // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); @@ -59,11 +56,11 @@ void Copter::brake_run() wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); // body-frame rate controller is run directly from 100hz loop // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(0.0f, G_Dt, false); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index e28ad52342..9b7b45e9f0 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -78,6 +78,6 @@ void Copter::circle_run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 666345666c..6001e7f396 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -104,7 +104,7 @@ void Copter::loiter_run() } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 178bd122c9..66946ca081 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -510,7 +510,7 @@ void Copter::poshold_run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index fba8ae8642..c0fabd4345 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -101,7 +101,7 @@ void Copter::sport_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); }