Refactor servo to be ready to in-flight config changes

Summary:
To make Servo ready to change ki and kp values during operation, we have to factorize some bits.

For now we will have 2 modes only - High converge (more aggressive) and Low converge (less aggressive)

This diff has no functional changes and have all tests green

Reviewed By: leoleovich

Differential Revision: D66707981

fbshipit-source-id: 943477db63516b5fe9fc0b15e2f606d0853ed300

servo/pi.go

@@ -25,9 +25,14 @@ import (
 )
 
 const (
+	// kp and ki scale for high offset range - more aggressive servo
 	kpScale = 0.7
 	kiScale = 0.3
 
+	// kp and ki scale for low offset range - less aggressive servo
+	kpScaleLow = 0.07
+	kiScaleLow = 0.03
+
 	maxKpNormMax = 1.0
 	maxKiNormMax = 2.0
 
@@ -98,6 +103,7 @@ type PiServo struct {
 	kp                 float64
 	ki                 float64
 	lastFreq           float64
+	syncInterval       float64
 	count              int
 	lastCorrectionTime time.Time
 	filter             *PiServoFilter
@@ -155,6 +161,8 @@ func (s *PiServo) IsSpike(offset int64) bool {
 		s.count = 0
 		s.drift = 0
 		s.filter.Reset() // it's safe because fState can only be filterNoSpike without filter
+		s.cfg.makePiFast()
+		s.resyncInterval()
 		log.Warning("servo was reset")
 		return true
 	}
@@ -253,19 +261,27 @@ func (s *PiServo) Sample(offset int64, localTs uint64) (float64, State) {
 	return ppb, state
 }
 
-// SyncInterval inform a clock servo about the master's sync interval in seconds
-func (s *PiServo) SyncInterval(interval float64) {
-	s.kp = s.cfg.PiKpScale * math.Pow(interval, s.cfg.PiKpExponent)
-	if s.kp > s.cfg.PiKpNormMax/interval {
-		s.kp = s.cfg.PiKpNormMax / interval
+func (s *PiServo) resyncInterval() {
+	if s.syncInterval == 0 {
+		return
+	}
+	s.kp = s.cfg.PiKpScale * math.Pow(s.syncInterval, s.cfg.PiKpExponent)
+	if s.kp > s.cfg.PiKpNormMax/s.syncInterval {
+		s.kp = s.cfg.PiKpNormMax / s.syncInterval
 	}
 
-	s.ki = s.cfg.PiKiScale * math.Pow(interval, s.cfg.PiKiExponent)
-	if s.ki > s.cfg.PiKiNormMax/interval {
-		s.ki = s.cfg.PiKiNormMax / interval
+	s.ki = s.cfg.PiKiScale * math.Pow(s.syncInterval, s.cfg.PiKiExponent)
+	if s.ki > s.cfg.PiKiNormMax/s.syncInterval {
+		s.ki = s.cfg.PiKiNormMax / s.syncInterval
 	}
 }
 
+// SyncInterval inform a clock servo about the master's sync interval in seconds
+func (s *PiServo) SyncInterval(interval float64) {
+	s.syncInterval = interval
+	s.resyncInterval()
+}
+
 // GetState returns current state of PiServo
 func (s *PiServo) GetState() State {
 	switch s.count {
@@ -387,6 +403,8 @@ func (f *PiServoFilter) Sample(s *PiServoFilterSample) {
 // Unlock resets and unlocks the servo
 func (s *PiServo) Unlock() {
 	s.count = 0
+	s.cfg.makePiFast()
+	s.resyncInterval()
 	s.filter.Reset()
 }
 
@@ -442,18 +460,28 @@ func NewPiServoFilter(s *PiServo, cfg *PiServoFilterCfg) *PiServoFilter {
 	return filter
 }
 
+func (cfg *PiServoCfg) makePiFast() {
+	cfg.PiKpScale = kpScale
+	cfg.PiKiScale = kiScale
+}
+
+func (cfg *PiServoCfg) makePiSlow() {
+	cfg.PiKpScale = kpScaleLow
+	cfg.PiKiScale = kiScaleLow
+}
+
 // DefaultPiServoCfg to create default pi servo config
 func DefaultPiServoCfg() *PiServoCfg {
-	return &PiServoCfg{
+	cfg := PiServoCfg{
 		PiKp:         0.0,
 		PiKi:         0.0,
-		PiKpScale:    kpScale,
 		PiKpExponent: 0.0,
 		PiKpNormMax:  maxKpNormMax,
-		PiKiScale:    kiScale,
 		PiKiExponent: 0.0,
 		PiKiNormMax:  maxKiNormMax,
 	}
+	cfg.makePiFast()
+	return &cfg
 }
 
 // DefaultPiServoFilterCfg to create a default pi servo filter config

servo/pi_test.go

@@ -594,3 +594,22 @@ func TestPiServoFilterSample2(t *testing.T) {
 	freq = pi.MeanFreq()
 	require.InEpsilon(t, -23197.000, freq, 0.001)
 }
+
+func TestPiServoAltMode(t *testing.T) {
+	pi := NewPiServo(DefaultServoConfig(), DefaultPiServoCfg(), 0)
+	pi.SyncInterval(1)
+
+	pi.cfg.makePiSlow()
+	pi.resyncInterval()
+	require.InEpsilon(t, 0.03, pi.cfg.PiKiScale, 0.00001)
+	require.InEpsilon(t, 0.07, pi.cfg.PiKpScale, 0.00001)
+	require.InEpsilon(t, 0.03, pi.ki, 0.001)
+	require.InEpsilon(t, 0.07, pi.kp, 0.00001)
+
+	pi.cfg.makePiFast()
+	pi.resyncInterval()
+	require.InEpsilon(t, 0.3, pi.cfg.PiKiScale, 0.00001)
+	require.InEpsilon(t, 0.7, pi.cfg.PiKpScale, 0.00001)
+	require.InEpsilon(t, 0.3, pi.ki, 0.00001)
+	require.InEpsilon(t, 0.7, pi.kp, 0.00001)
+}