[RSDK-6164] Mimo pid updates

control/control_loop.go

@@ -330,17 +330,29 @@ func (l *Loop) GetConfig(ctx context.Context) Config {
 func (l *Loop) MonitorTuning(ctx context.Context) {
 	// wait until tuning has started
 	for {
-		tuning := l.GetTuning(ctx)
-		if tuning {
-			break
+		// 100 Hz is probably faster than we need, but we needed at least a small delay because
+		// GetTuning will lock the PID block
+		if utils.SelectContextOrWait(ctx, 10*time.Millisecond) {
+			tuning := l.GetTuning(ctx)
+			if tuning {
+				break
+			}
+			continue
 		}
+		l.logger.Error("error starting tuner")
+		return
 	}
 	// wait until tuning is done
 	for {
-		tuning := l.GetTuning(ctx)
-		if !tuning {
-			break
+		if utils.SelectContextOrWait(ctx, 10*time.Millisecond) {
+			tuning := l.GetTuning(ctx)
+			if !tuning {
+				break
+			}
+			continue
 		}
+		l.logger.Error("error waiting for tuner")
+		return
 	}
 }
 

control/control_signal.go

@@ -1,6 +1,8 @@
 package control
 
-import "sync"
+import (
+	"sync"
+)
 
 // Signal holds any data passed between blocks.
 type Signal struct {
@@ -23,11 +25,23 @@ func makeSignal(name string, blockType controlBlockType) *Signal {
 	return &s
 }
 
+// Make Signals returns a Signal object where the length of its signal[] array is dependent
+// on the number of PIDSets from the config.
+func makeSignals(name string, blockType controlBlockType, dimension int) *Signal {
+	var s Signal
+	s.dimension = dimension
+	s.signal = make([]float64, dimension)
+	s.time = make([]int, dimension)
+	s.name = name
+	s.blockType = blockType
+	return &s
+}
+
 // GetSignalValueAt returns the value of the signal at an index, threadsafe.
 func (s *Signal) GetSignalValueAt(i int) float64 {
 	s.mu.Lock()
 	defer s.mu.Unlock()
-	if i > len(s.signal)-1 {
+	if !(i < len(s.signal)) {
 		return 0.0
 	}
 	return s.signal[i]
@@ -37,7 +51,7 @@ func (s *Signal) GetSignalValueAt(i int) float64 {
 func (s *Signal) SetSignalValueAt(i int, val float64) {
 	s.mu.Lock()
 	defer s.mu.Unlock()
-	if i > len(s.signal)-1 {
+	if !(i < len(s.signal)) {
 		return
 	}
 	s.signal[i] = val