Improve accessory turn-off logic in arducontrol driver

[r2c2.git] / source / libr2c2 / arducontrol.cpp

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 7b5fc108f09bf0864776c67b0a9ce7f717edd4c7..3fc9f577e9cc5106d170f8a4ad586ea24fd8d2fe 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -208,7 +208,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
                throw invalid_argument("ArduControl::set_loco_function");
 
        unsigned mask = 1<<func;
-       if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
+       if(loco.funcs.set((loco.funcs.pending&~mask)|(mask*state)))
        {
                if(func>0 || loco.proto!=MM)
                {
@@ -503,9 +503,11 @@ void ArduControl::tick()
 
        if(active_accessory && off_timeout)
        {
-               bool success = (monitor.get_peak()>0.35f && monitor.get_current()<monitor.get_peak()-0.2f);
+               bool got_peak = monitor.get_peak()>0.0f;
+               bool success = monitor.get_peak()>0.42f;
+               bool complete = (success && monitor.get_current()<monitor.get_peak()-0.2f);
                Time::TimeStamp t = Time::now();
-               if(t>off_timeout || success)
+               if((t>off_timeout && got_peak) || complete)
                {
                        Accessory &acc = *active_accessory;