From 808efa4c793e1745b6051ea8bbbd5747bda228c9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 22 Aug 2019 14:22:30 +0900 Subject: [PATCH] Copter: surface tracking format fixes --- ArduCopter/Copter.h | 16 +++++++--------- ArduCopter/surface_tracking.cpp | 25 ++++++++++++++----------- 2 files changed, 21 insertions(+), 20 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 11b49c079e..5084c22cc1 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -293,18 +293,16 @@ private: class SurfaceTracking { public: + // get desired climb rate (in cm/s) to achieve surface tracking float adjust_climb_rate(float target_rate); + + // get/set target altitude (in cm) above ground bool get_target_alt_cm(float &target_alt_cm) const; void set_target_alt_cm(float target_alt_cm); - float logging_target_alt() const { - if (!valid_for_logging) { - return AP::logger().quiet_nan(); - } - return target_alt_cm * 0.01f; // cm->m - } - void invalidate_for_logging() { - valid_for_logging = false; - } + + // get target altitude (in cm) for logging purposes + float logging_target_alt() const; + void invalidate_for_logging() { valid_for_logging = false; } private: float target_alt_cm; // desired altitude in cm above the ground diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index dd7be9466d..a7f77ce15e 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -13,14 +13,9 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) const float current_alt = copter.inertial_nav.get_altitude(); const float current_alt_target = copter.pos_control->get_alt_target(); - float distance_error; - float velocity_correction; - - uint32_t now = millis(); - - valid_for_logging = true; // reset target altitude if this controller has just been engaged + const uint32_t now = millis(); if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt; } @@ -30,6 +25,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) { target_alt_cm += target_rate * copter.G_Dt; } + valid_for_logging = true; /* handle rangefinder glitches. When we get a rangefinder reading @@ -39,7 +35,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) row. When that happens we reset the target altitude to the new reading */ - int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm; + const int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm; if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { copter.rangefinder_state.glitch_count = MAX(copter.rangefinder_state.glitch_count+1,1); } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) { @@ -58,8 +54,8 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) } // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) - distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt); - velocity_correction = distance_error * copter.g.rangefinder_gain; + const float distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt); + float velocity_correction = distance_error * copter.g.rangefinder_gain; velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX); // return combined pilot climb rate + rate to correct rangefinder alt error @@ -69,7 +65,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) #endif } -// get surfacing tracking alt +// get target altitude (in cm) above ground // returns true if there is a valid target bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const { @@ -81,10 +77,17 @@ bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const return true; } -// set surface tracking target altitude +// set target altitude (in cm) above ground void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm) { target_alt_cm = _target_alt_cm; last_update_ms = AP_HAL::millis(); } +float Copter::SurfaceTracking::logging_target_alt() const +{ + if (!valid_for_logging) { + return AP::logger().quiet_nan(); + } + return target_alt_cm * 0.01f; // cm->m +}