diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 43d15f6dda..4f6d28b6fe 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -118,17 +118,6 @@ float Copter::get_non_takeoff_throttle() return MAX(0,motors->get_throttle_hover()/2.0f); } -// get target climb rate reduced to avoid obstacles and altitude fence -float Copter::get_avoidance_adjusted_climbrate(float target_rate) -{ -#if AC_AVOID_ENABLED == ENABLED - avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), target_rate, G_Dt); - return target_rate; -#else - return target_rate; -#endif -} - // set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle void Copter::set_accel_throttle_I_from_pilot_throttle() { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7e70dfd5de..5a44e67e35 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -662,7 +662,6 @@ private: void update_throttle_hover(); float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); - float get_avoidance_adjusted_climbrate(float target_rate); void set_accel_throttle_I_from_pilot_throttle(); void rotate_body_frame_to_NE(float &x, float &y); uint16_t get_pilot_speed_dn(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5a71cf7787..580521e1d3 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -718,6 +718,16 @@ float Mode::get_pilot_desired_throttle() const return throttle_out; } +float Mode::get_avoidance_adjusted_climbrate(float target_rate) +{ +#if AC_AVOID_ENABLED == ENABLED + AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), target_rate, G_Dt); + return target_rate; +#else + return target_rate; +#endif +} + Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms) { // Alt Hold State Machine Determination @@ -815,11 +825,6 @@ void Mode::set_throttle_takeoff() pos_control->init_takeoff(); } -float Mode::get_avoidance_adjusted_climbrate(float target_rate) -{ - return copter.get_avoidance_adjusted_climbrate(target_rate); -} - uint16_t Mode::get_pilot_speed_dn() { return copter.get_pilot_speed_dn(); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 0fc8ad549b..2d94ebdaf2 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -86,6 +86,10 @@ public: float get_pilot_desired_yaw_rate(int16_t stick_angle); float get_pilot_desired_throttle() const; + // returns climb target_rate reduced to avoid obstacles and + // altitude fence + float get_avoidance_adjusted_climbrate(float target_rate); + const Vector3f& get_desired_velocity() { // note that position control isn't used in every mode, so // this may return bogus data: @@ -245,7 +249,6 @@ public: void set_land_complete(bool b); GCS_Copter &gcs(); void set_throttle_takeoff(void); - float get_avoidance_adjusted_climbrate(float target_rate); uint16_t get_pilot_speed_dn(void); // end pass-through functions diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 7b77e0ab32..063e3e8a63 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -84,7 +84,7 @@ float AutoTune::get_pilot_desired_climb_rate_cms(void) const float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()); // get avoidance adjusted climb rate - target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); + target_climb_rate = copter.mode_autotune.get_avoidance_adjusted_climbrate(target_climb_rate); return target_climb_rate; } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 50879c6716..946f3cd704 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -286,7 +286,7 @@ void ModeFlowHold::run() takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate - target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // call position controller copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false); @@ -309,7 +309,7 @@ void ModeFlowHold::run() target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); // get avoidance adjusted climb rate - target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); break;