• Alessandro Rubini's avatar
    servo: average ofm like we do for owd, with some care · 5927cf03
    Alessandro Rubini authored
    In my opinion, it makes no sense to make calculations with a
    well-filtered OWD and the instantaneous OFM, which is subject to the
    same oscillations.  Thus, this commit filters OFM with the same rules
    used for OWD.
    
    However, the detection of outliers is more difficult because OFM can
    have either sign, and OFM is really changing while we are not yet
    synchronized.  So the code does the following:
    
    - if we are at PP_ADJ_FREQ_MAX it means we are bridging a gap as
    fast as possible. In that case, don't average OFM.
    
    - if we receive a bigger-than-expected OFM value, trim it, but relax
    the expectation so we can track a real change if futher samples confirm
    it or push it farther.
    
    - if the averaged OFM changed sign, start averaging from scratch.
    
    The second item works by keeping the average of ofm magnitude, and we
    accept a change no bigger than the averaged magnitude.  So, if we are
    synced in the millisecond range we accept changes of the order of
    milliseconds (this usually applies while syncing, and OFM is expected
    to change in that case); if we are in the microsecond range outliers
    move us by a few microseconds, but such trimming is then relaxed so we
    increase our ability to move exponentially.
    
    The last item prevents oscillations: this OFM is fed to a PI controller,
    so if we are late in responding to changes in sign, both P and I
    continue pushing in the wrong direction. So, combining the average and
    the PI we have an almost inversion in phase of the error, and the
    servo oscillates a lot while converging.
    
    Unfortunately, while working on this I realized there's a bug when
    dealing with very short and consistent timestamps. For example, a
    current average of 20ns, will never reach 40ns even if all further
    samples request 40ns, due to integer truncation.  We need to either
    move to floating point or count fractional bits as the magnitudes
    permit.
    Signed-off-by: Alessandro Rubini's avatarAlessandro Rubini <rubini@gnudd.com>
    5927cf03
servo.c 10.1 KB