From adc1d60ea51d8fe9927b9fde982bf45f6fe549fe Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 11 Nov 2019 12:55:43 +0900
Subject: [PATCH] Copter: CTUN logging fix for SAlt

sonar altitude was not being logged in modes that don't use surface tracking including Auto
SAlt scaling was also incorrect
---
 ArduCopter/Copter.h             | 3 ++-
 ArduCopter/Log.cpp              | 9 ++++-----
 ArduCopter/surface_tracking.cpp | 9 +++++++--
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 77882f2af3..6ca1515564 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -286,7 +286,8 @@ private:
         void set_target_alt_cm(float target_alt_cm);
 
         // get target and actual distances (in m) for logging purposes
-        bool get_dist_for_logging(float &target_dist, float &actual_dist) const;
+        bool get_target_dist_for_logging(float &target_dist) const;
+        float get_dist_for_logging() const;
         void invalidate_for_logging() { valid_for_logging = false; }
 
         // surface tracking surface
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index f1f58f82f4..a4e335e854 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -41,10 +41,9 @@ void Copter::Log_Write_Control_Tuning()
     }
 
     // get surface tracking alts
-    float desired_rangefinder_alt, rangefinder_alt;
-    if (!surface_tracking.get_dist_for_logging(desired_rangefinder_alt, rangefinder_alt)) {
+    float desired_rangefinder_alt;
+    if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) {
         desired_rangefinder_alt = AP::logger().quiet_nan();
-        rangefinder_alt = AP::logger().quiet_nan();;
     }
 
     struct log_Control_Tuning pkt = {
@@ -58,7 +57,7 @@ void Copter::Log_Write_Control_Tuning()
         inav_alt            : inertial_nav.get_altitude() / 100.0f,
         baro_alt            : baro_alt,
         desired_rangefinder_alt : desired_rangefinder_alt,
-        rangefinder_alt     : rangefinder_alt,
+        rangefinder_alt     : surface_tracking.get_dist_for_logging(),
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,
         climb_rate          : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t
@@ -457,7 +456,7 @@ const struct LogStructure Copter::log_structure[] = {
     { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
       "PTUN", "QBfff",         "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
-      "CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B0B0BB-" },
+      "CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B000BB-" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
       "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
     { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp
index 0c5c497a17..65cfd08115 100644
--- a/ArduCopter/surface_tracking.cpp
+++ b/ArduCopter/surface_tracking.cpp
@@ -86,16 +86,21 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
     last_update_ms = AP_HAL::millis();
 }
 
-bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const
+bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
 {
     if (!valid_for_logging || (surface == Surface::NONE)) {
         return false;
     }
+
     target_dist = target_dist_cm * 0.01f;
-    actual_dist = ((surface == Surface::GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
     return true;
 }
 
+float Copter::SurfaceTracking::get_dist_for_logging() const
+{
+    return ((surface == Surface::CEILING) ? copter.rangefinder_up_state.alt_cm : copter.rangefinder_state.alt_cm) * 0.01f;
+}
+
 // set direction
 void Copter::SurfaceTracking::set_surface(Surface new_surface)
 {