From 62a4867cd428bfb7c1bc225faecd31de748a4970 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 5 Jan 2017 15:24:02 +0900 Subject: [PATCH] Copter: use avoidance adjusted climb rate in all modes --- ArduCopter/control_althold.cpp | 6 ++++++ ArduCopter/control_autotune.cpp | 3 +++ ArduCopter/control_guided.cpp | 3 +++ ArduCopter/control_loiter.cpp | 6 ++++++ ArduCopter/control_poshold.cpp | 4 ++++ ArduCopter/control_sport.cpp | 6 ++++++ 6 files changed, 28 insertions(+) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 1a314816e3..e22c2150c1 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -108,6 +108,9 @@ void Copter::althold_run() // get take-off adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); @@ -149,6 +152,9 @@ void Copter::althold_run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index d6f587e408..fa489c518e 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -285,6 +285,9 @@ void Copter::autotune_run() // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // check for pilot requested take-off - this should not actually be possible because of autotune_init() checks if (ap.land_complete && target_climb_rate > 0) { // indicate we are taking off diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 59ae772069..fe61acd87d 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -580,6 +580,9 @@ void Copter::guided_angle_control_run() // constrain climb rate float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_speed_down()), wp_nav->get_speed_up()); + // get avoidance adjusted climb rate + climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); + // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 53c0542b85..4e8eb9d159 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -162,6 +162,9 @@ void Copter::loiter_run() // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); @@ -212,6 +215,9 @@ void Copter::loiter_run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index bbf986ab2f..2ff9b19261 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -538,6 +538,10 @@ void Copter::poshold_run() // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } + + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 3c56b8c3c2..492abb22b2 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -119,6 +119,9 @@ void Copter::sport_run() // get take-off adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // call attitude controller attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); @@ -154,6 +157,9 @@ void Copter::sport_run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller();