From b7d0e4ec1078ddaddf0ce2b23d8f2320450e0a80 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 Apr 2019 21:14:08 +0900 Subject: [PATCH] Copter: remove surface tracking shim functions --- ArduCopter/mode.cpp | 16 ---------------- ArduCopter/mode.h | 3 --- ArduCopter/mode_althold.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- ArduCopter/mode_loiter.cpp | 2 +- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_sport.cpp | 2 +- ArduCopter/mode_zigzag.cpp | 6 +++--- 9 files changed, 9 insertions(+), 28 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4856cfbadb..1b311fa2ba 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -687,22 +687,6 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. - -float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate) -{ - return copter.get_surface_tracking_climb_rate(target_rate); -} - -bool Copter::Mode::get_surface_tracking_target_alt_cm(float &target_alt_cm) const -{ - return copter.get_surface_tracking_target_alt_cm(target_alt_cm); -} - -void Copter::Mode::set_surface_tracking_target_alt_cm(float target_alt_cm) -{ - copter.set_surface_tracking_target_alt_cm(target_alt_cm); -} - float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) { return copter.get_pilot_desired_yaw_rate(stick_angle); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index e4838532a1..19a2a1f8b9 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -197,9 +197,6 @@ 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); - bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const; - void set_surface_tracking_target_alt_cm(float target_alt_cm); 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 f7017b3777..e06992ef00 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); + target_climb_rate = copter.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 2db7079b31..0b4345413b 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); + target_climb_rate = copter.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 0e206a1230..af221c23f7 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 = get_surface_tracking_climb_rate(target_climb_rate); + target_climb_rate = copter.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 e9f8693f3e..c56cf4541c 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); + target_climb_rate = copter.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 321f56805f..aaed8615e2 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); + target_climb_rate = copter.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 e46353a08e..4f194b0e09 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); + target_climb_rate = copter.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 3091fea4c3..558ad03ee2 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -125,7 +125,7 @@ void Copter::ModeZigZag::return_to_manual_control(bool maintain_target) const Vector3f wp_dest = wp_nav->get_wp_destination(); loiter_nav->init_target(wp_dest); if (maintain_target && wp_nav->origin_and_destination_are_terrain_alt()) { - set_surface_tracking_target_alt_cm(wp_dest.z); + copter.set_surface_tracking_target_alt_cm(wp_dest.z); } gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); } @@ -200,7 +200,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); + target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); @@ -283,7 +283,7 @@ bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_al // if we have a downward facing range finder then use terrain altitude targets terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used(); if (terrain_alt) { - if (!get_surface_tracking_target_alt_cm(next_dest.z)) { + if (!copter.get_surface_tracking_target_alt_cm(next_dest.z)) { next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else {