diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index bf9c5a49cb..81324bb70e 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -121,7 +121,7 @@ float Copter::get_non_takeoff_throttle() // get_surface_tracking_climb_rate - hold copter at the desired distance above the ground // returns climb rate (in cm/s) which should be passed to the position controller -float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) +float Copter::get_surface_tracking_climb_rate(int16_t target_rate) { #if RANGEFINDER_ENABLED == ENABLED if (!copter.rangefinder_alt_ok()) { @@ -130,9 +130,10 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current } static uint32_t last_call_ms = 0; + const float current_alt = inertial_nav.get_altitude(); + const float current_alt_target = pos_control->get_alt_target(); float distance_error; float velocity_correction; - float current_alt = inertial_nav.get_altitude(); uint32_t now = millis(); @@ -146,7 +147,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current // adjust rangefinder target alt if motors have not hit their limits if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) { - target_rangefinder_alt += target_rate * dt; + target_rangefinder_alt += target_rate * G_Dt; } /* diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bc7f179de5..787fc08dfa 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -617,7 +617,7 @@ private: void set_throttle_takeoff(); float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); - float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); + float get_surface_tracking_climb_rate(int16_t target_rate); 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); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 564e42e752..4bd96d942f 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -688,9 +688,9 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli // these are candidates for moving into the Mode base // class. -float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) +float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate) { - return copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt); + return copter.get_surface_tracking_climb_rate(target_rate); } float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 403171c03e..984113fafb 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -197,7 +197,7 @@ protected: // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. - float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); + float get_surface_tracking_climb_rate(int16_t target_rate); float get_pilot_desired_yaw_rate(int16_t stick_angle); float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_throttle() const; diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 34b5b9b9a0..f7017b3777 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -96,7 +96,7 @@ void Copter::ModeAltHold::run() #endif // adjust climb rate using rangefinder - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index f24bf258bd..2db7079b31 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -44,7 +44,7 @@ void Copter::ModeCircle::run() // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // 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); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); } // if not armed set throttle to zero and exit immediately diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 79bde883ea..0e206a1230 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -300,7 +300,7 @@ void Copter::ModeFlowHold::run() copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // adjust climb rate using rangefinder - target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 7739f929f5..e9f8693f3e 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -187,7 +187,7 @@ void Copter::ModeLoiter::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // adjust climb rate using rangefinder - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 9a68e9ffed..321f56805f 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -234,7 +234,7 @@ void Copter::ModePosHold::run() // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // 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); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); } // get avoidance adjusted climb rate diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 0f53aaffe6..e46353a08e 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -121,7 +121,7 @@ void Copter::ModeSport::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // adjust climb rate using rangefinder - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index c7001475a9..7149645412 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -192,7 +192,7 @@ void Copter::ModeZigZag::manual_control() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // adjust climb rate using rangefinder - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);