diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 5147f03ee5..3e51c85770 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -106,6 +106,7 @@ void AP_AutoTune::start(void) state_enter_ms = now; last_save_ms = now; + last_save = current; restore = current; /* @@ -246,12 +247,14 @@ void AP_AutoTune::check_save(void) */ void AP_AutoTune::save_gains(const ATGains &v) { + current = last_save; current.tau.set_and_save_ifchanged(v.tau); current.P.set_and_save_ifchanged(v.P); current.I.set_and_save_ifchanged(v.I); current.D.set_and_save_ifchanged(v.D); current.rmax.set_and_save_ifchanged(v.rmax); current.imax.set_and_save_ifchanged(v.imax); + last_save = current; } #define LOG_MSG_ATRP 211 diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 4fbd0223ae..510a03601d 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -58,6 +58,9 @@ private: // values to restore if we leave autotune mode ATGains restore; + // values we last saved + ATGains last_save; + // values to save on the next save event ATGains next_save;