From eb1fb63e5771e0d7bf016ad3874c78bdb65786f0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 30 Apr 2014 22:22:13 +1000
Subject: [PATCH] APM_Control: added logging of parameter changes in AUTOTUNE

this will make it easier to analyse logs
---
 libraries/APM_Control/AP_AutoTune.cpp | 49 +++++++++++++++++++++++----
 libraries/APM_Control/AP_AutoTune.h   |  4 ++-
 2 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp
index 1b0d759199..fc082996d1 100644
--- a/libraries/APM_Control/AP_AutoTune.cpp
+++ b/libraries/APM_Control/AP_AutoTune.cpp
@@ -260,31 +260,66 @@ void AP_AutoTune::check_save(void)
     last_save_ms = hal.scheduler->millis();
 }
 
+/*
+  log a parameter change from autotune
+ */
+void AP_AutoTune::log_param_change(float v, const prog_char_t *suffix)
+{
+    if (!dataflash.logging_started()) {
+        return;
+    }
+    char key[AP_MAX_NAME_SIZE+1];
+    if (type == AUTOTUNE_ROLL) {
+        strncpy_P(key, PSTR("RLL2SRV_"), 8);
+        strncpy_P(&key[8], suffix, AP_MAX_NAME_SIZE-8);
+    } else {
+        strncpy_P(key, PSTR("PTCH2SRV_"), 9);
+        strncpy_P(&key[9], suffix, AP_MAX_NAME_SIZE-9);
+    }
+    key[AP_MAX_NAME_SIZE] = 0;
+    dataflash.Log_Write_Parameter(key, v);
+}
+
 /*
   set a float and save a float if it has changed by more than
   0.1%. This reduces the number of insignificant EEPROM writes
  */
-void AP_AutoTune::save_float_if_changed(AP_Float &v, float value)
+void AP_AutoTune::save_float_if_changed(AP_Float &v, float value, const prog_char_t *suffix)
 {
     float old_value = v.get();
     v.set(value);
     if (value <= 0 || fabsf((value-old_value)/value) > 0.001f) {
         v.save();
+        log_param_change(v.get(), suffix);
     }
 }
 
+/*
+  set a int16 and save if changed
+ */
+void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t value, const prog_char_t *suffix)
+{
+    int16_t old_value = v.get();
+    v.set(value);
+    if (old_value != v.get()) {
+        v.save();
+        log_param_change(v.get(), suffix);
+    }
+}
+
+
 /*
   save a set of gains
  */
 void AP_AutoTune::save_gains(const ATGains &v)
 {
     current = last_save;
-    save_float_if_changed(current.tau, v.tau);
-    save_float_if_changed(current.P, v.P);
-    save_float_if_changed(current.I, v.I);
-    save_float_if_changed(current.D, v.D);
-    current.rmax.set_and_save_ifchanged(v.rmax);
-    current.imax.set_and_save_ifchanged(v.imax);
+    save_float_if_changed(current.tau, v.tau, PSTR("TCONST"));
+    save_float_if_changed(current.P, v.P, PSTR("P"));
+    save_float_if_changed(current.I, v.I, PSTR("I"));
+    save_float_if_changed(current.D, v.D, PSTR("D"));
+    save_int16_if_changed(current.rmax, v.rmax, PSTR("RMAX"));
+    save_int16_if_changed(current.imax, v.imax, PSTR("IMAX"));
     last_save = current;
 }
 
diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h
index fd7b60ff14..4e286394e6 100644
--- a/libraries/APM_Control/AP_AutoTune.h
+++ b/libraries/APM_Control/AP_AutoTune.h
@@ -95,7 +95,9 @@ private:
     void write_log_headers(void);
     void write_log(float servo, float demanded, float achieved);
 
-    void save_float_if_changed(AP_Float &v, float value);
+    void log_param_change(float v, const prog_char_t *suffix);
+    void save_float_if_changed(AP_Float &v, float value, const prog_char_t *suffix);
+    void save_int16_if_changed(AP_Int16 &v, int16_t value, const prog_char_t *suffix);        
 };
 
 #endif // __AP_AUTOTUNE_H__