feat: update led on speed zone

cmd/rc-led/rc-led.go

@@ -15,7 +15,7 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var driveModeTopic, recordTopic string
+	var driveModeTopic, recordTopic, speedZoneTopic string
 
 	mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
 	_, mqttRetain := os.LookupEnv("MQTT_RETAIN")
@@ -24,6 +24,7 @@ func main() {
 
 	flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set")
 	flag.StringVar(&recordTopic, "mqtt-topic-record", os.Getenv("MQTT_TOPIC_RECORD"), "Mqtt topic that contains video recording state, use MQTT_TOPIC_RECORD if args not set")
+	flag.StringVar(&speedZoneTopic, "mqtt-topic-speed-zone", os.Getenv("MQTT_TOPIC_SPEED_ZONE"), "Mqtt topic that contains speed zone, use MQTT_TOPIC_SPEED_ZONE if args not set")
 
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 	flag.Parse()
@@ -52,7 +53,7 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	p := part.NewPart(client, driveModeTopic, recordTopic)
+	p := part.NewPart(client, driveModeTopic, recordTopic, speedZoneTopic)
 	defer p.Stop()
 
 	cli.HandleExit(p)

pkg/led/led.go

@@ -19,10 +19,12 @@ func init() {
 }
 
 var (
-	ColorBlack = Color{0, 0, 0}
+	ColorBlack  = Color{0, 0, 0}
-	ColorRed   = Color{255, 0, 0}
+	ColorRed    = Color{255, 0, 0}
-	ColorGreen = Color{0, 255, 0}
+	ColorYellow = Color{255, 255, 0}
-	ColorBlue  = Color{0, 0, 255}
+	ColorGreen  = Color{0, 255, 0}
+	ColorBlue   = Color{0, 0, 255}
+	ColorWhite  = Color{255, 255, 255}
 )
 
 func New() *PiColorLed {

pkg/led/led_test.go

@@ -11,27 +11,38 @@ func TestColorLed_Red(t *testing.T) {
 	setLedBackup := setLed
 	defer func() { setLed = setLedBackup }()
 
-	ledColors := make(map[gpio.PinIO]int)
+	l := New()
+	fakeLed := struct {
+		redValue   int
+		greenValue int
+		blueValue  int
+	}{}
 	setLed = func(v int, led gpio.PinIO, mutex *sync.Mutex) {
 		mutex.Lock()
 		defer mutex.Unlock()
-		ledColors[led] = v
+		switch led {
+		case l.pinRed:
+			fakeLed.redValue = v
+		case l.pinGreen:
+			fakeLed.greenValue = v
+		case l.pinBlue:
+			fakeLed.blueValue = v
+		}
 	}
 
-	l := New()
 	if l.Red() != 0 {
 		t.Errorf("%T.Red(): %v, wants %v", l, l.Red(), 0)
 	}
-	if ledColors[l.pinRed] != 0 {
+	if fakeLed.redValue != 0 {
-		t.Errorf("colorValue: %v, wants %v", ledColors[l.pinRed], 0)
+		t.Errorf("colorValue: %v, wants %v", fakeLed.redValue, 0)
 	}
 
 	l.SetColor(ColorRed)
 	if l.Red() != 255 {
 		t.Errorf("%T.Red(): %v, wants %v", l, l.Red(), 255)
 	}
-	if ledColors[l.pinRed] != 255 {
+	if fakeLed.redValue != 255 {
-		t.Errorf("colorValue: %v, wants %v", ledColors[l.pinRed], 255)
+		t.Errorf("colorValue: %v, wants %v", fakeLed.redValue, 255)
 	}
 }
 
@@ -39,27 +50,38 @@ func TestColorLed_Green(t *testing.T) {
 	setLedBackup := setLed
 	defer func() { setLed = setLedBackup }()
 
-	ledColors := make(map[gpio.PinIO]int)
+	l := New()
+	fakeLed := struct {
+		redValue   int
+		greenValue int
+		blueValue  int
+	}{}
 	setLed = func(v int, led gpio.PinIO, mutex *sync.Mutex) {
 		mutex.Lock()
 		defer mutex.Unlock()
-		ledColors[led] = v
+		switch led {
+		case l.pinRed:
+			fakeLed.redValue = v
+		case l.pinGreen:
+			fakeLed.greenValue = v
+		case l.pinBlue:
+			fakeLed.blueValue = v
+		}
 	}
 
-	l := New()
 	if l.Green() != 0 {
 		t.Errorf("%T.Green(): %v, wants %v", l, l.Green(), 0)
 	}
-	if ledColors[l.pinGreen] != 0 {
+	if fakeLed.greenValue != 0 {
-		t.Errorf("colorValue: %v, wants %v", ledColors[l.pinGreen], 0)
+		t.Errorf("colorValue: %v, wants %v", fakeLed.greenValue, 0)
 	}
 
 	l.SetColor(ColorGreen)
 	if l.Green() != 255 {
 		t.Errorf("%T.Green(): %v, wants %v", l, l.Green(), 255)
 	}
-	if ledColors[l.pinGreen] != 255 {
+	if fakeLed.greenValue != 255 {
-		t.Errorf("colorValue: %v, wants %v", ledColors[l.pinGreen], 255)
+		t.Errorf("colorValue: %v, wants %v", fakeLed.greenValue, 255)
 	}
 }
 
@@ -67,27 +89,38 @@ func TestColorLed_Blue(t *testing.T) {
 	setLedBackup := setLed
 	defer func() { setLed = setLedBackup }()
 
-	ledColors := make(map[gpio.PinIO]int)
+	l := New()
+	fakeLed := struct {
+		redValue   int
+		greenValue int
+		blueValue  int
+	}{}
 	setLed = func(v int, led gpio.PinIO, mutex *sync.Mutex) {
 		mutex.Lock()
 		defer mutex.Unlock()
-		ledColors[led] = v
+		switch led {
+		case l.pinRed:
+			fakeLed.redValue = v
+		case l.pinGreen:
+			fakeLed.greenValue = v
+		case l.pinBlue:
+			fakeLed.blueValue = v
+		}
 	}
 
-	l := New()
 	if l.Blue() != 0 {
 		t.Errorf("%T.Blue(): %v, wants %v", l, l.Blue(), 0)
 	}
-	if ledColors[l.pinBlue] != 0 {
+	if fakeLed.blueValue != 0 {
-		t.Errorf("colorValue: %v, wants %v", ledColors[l.pinBlue], 0)
+		t.Errorf("colorValue: %v, wants %v", fakeLed.blueValue, 0)
 	}
 
 	l.SetColor(ColorBlue)
 	if l.Blue() != 255 {
 		t.Errorf("%T.Blue(): %v, wants %v", l, l.Blue(), 255)
 	}
-	if ledColors[l.pinBlue] != 255 {
+	if fakeLed.blueValue != 255 {
-		t.Errorf("colorValue: %v, wants %v", ledColors[l.pinBlue], 255)
+		t.Errorf("colorValue: %v, wants %v", fakeLed.blueValue, 255)
 	}
 }
 

pkg/part/part.go

@@ -12,16 +12,19 @@ import (
 	"time"
 )
 
-func NewPart(client mqtt.Client, driveModeTopic, recordTopic string) *LedPart {
+func NewPart(client mqtt.Client, driveModeTopic, recordTopic, speedZoneTopic string) *LedPart {
 	return &LedPart{
 		led:              led.New(),
 		client:           client,
 		onDriveModeTopic: driveModeTopic,
 		onRecordTopic:    recordTopic,
+		onSpeedZoneTopic: speedZoneTopic,
 		muDriveMode:      sync.Mutex{},
-		m:                events.DriveMode_INVALID,
+		driveMode:        events.DriveMode_INVALID,
 		muRecord:         sync.Mutex{},
 		recordEnabled:    false,
+		muSpeedZone:      sync.Mutex{},
+		speedZone:        events.SpeedZone_UNKNOWN,
 	}
 
 }
@@ -31,11 +34,15 @@ type LedPart struct {
 	client           mqtt.Client
 	onDriveModeTopic string
 	onRecordTopic    string
+	onSpeedZoneTopic string
 
 	muDriveMode   sync.Mutex
-	m             events.DriveMode
+	driveMode     events.DriveMode
 	muRecord      sync.Mutex
 	recordEnabled bool
+
+	muSpeedZone sync.Mutex
+	speedZone   events.SpeedZone
 }
 
 func (p *LedPart) Start() error {
@@ -50,7 +57,13 @@ func (p *LedPart) Start() error {
 func (p *LedPart) Stop() {
 	defer p.led.SetBlink(0)
 	defer p.led.SetColor(led.ColorBlack)
-	service.StopService("led", p.client, p.onDriveModeTopic, p.onRecordTopic)
+	service.StopService("led", p.client, p.onDriveModeTopic, p.onRecordTopic, p.onSpeedZoneTopic)
+}
+
+func (p *LedPart) setDriveMode(m events.DriveMode) {
+	p.muDriveMode.Lock()
+	defer p.muDriveMode.Unlock()
+	p.driveMode = m
 }
 
 func (p *LedPart) onDriveMode(_ mqtt.Client, message mqtt.Message) {
@@ -60,12 +73,8 @@ func (p *LedPart) onDriveMode(_ mqtt.Client, message mqtt.Message) {
 		zap.S().Errorf("unable to unmarshal %T message: %v", driveModeMessage, err)
 		return
 	}
-	switch driveModeMessage.GetDriveMode() {
+	p.setDriveMode(driveModeMessage.GetDriveMode())
-	case events.DriveMode_USER:
+	p.updateColor()
-		p.led.SetColor(led.ColorGreen)
-	case events.DriveMode_PILOT:
-		p.led.SetColor(led.ColorBlue)
-	}
 }
 
 func (p *LedPart) onRecord(client mqtt.Client, message mqtt.Message) {
@@ -92,6 +101,47 @@ func (p *LedPart) onRecord(client mqtt.Client, message mqtt.Message) {
 	}
 }
 
+func (p *LedPart) setSpeedZone(sz events.SpeedZone) {
+	p.muSpeedZone.Lock()
+	defer p.muSpeedZone.Unlock()
+	p.speedZone = sz
+}
+
+func (p *LedPart) onSpeedZone(_ mqtt.Client, message mqtt.Message) {
+	var speedZoneMessage events.SpeedZoneMessage
+	err := proto.Unmarshal(message.Payload(), &speedZoneMessage)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal %T message: %v", speedZoneMessage, err)
+		return
+	}
+
+	p.setSpeedZone(speedZoneMessage.GetSpeedZone())
+	p.updateColor()
+}
+
+func (p *LedPart) updateColor() {
+	p.muSpeedZone.Lock()
+	defer p.muSpeedZone.Unlock()
+	p.muDriveMode.Lock()
+	defer p.muDriveMode.Unlock()
+
+	switch p.driveMode {
+	case events.DriveMode_USER:
+		p.led.SetColor(led.ColorGreen)
+	case events.DriveMode_PILOT:
+		switch p.speedZone {
+		case events.SpeedZone_UNKNOWN:
+			p.led.SetColor(led.ColorWhite)
+		case events.SpeedZone_SLOW:
+			p.led.SetColor(led.ColorRed)
+		case events.SpeedZone_NORMAL:
+			p.led.SetColor(led.ColorYellow)
+		case events.SpeedZone_FAST:
+			p.led.SetColor(led.ColorBlue)
+		}
+	}
+}
+
 func (p *LedPart) registerCallbacks() error {
 	err := service.RegisterCallback(p.client, p.onDriveModeTopic, p.onDriveMode)
 	if err != nil {
@@ -102,5 +152,11 @@ func (p *LedPart) registerCallbacks() error {
 	if err != nil {
 		return err
 	}
+
+	err = service.RegisterCallback(p.client, p.onSpeedZoneTopic, p.onSpeedZone)
+	if err != nil {
+		return err
+	}
+
 	return nil
 }

pkg/part/part_test.go

@@ -29,7 +29,7 @@ func (f *fakeLed) SetBlink(freq float64) {
 
 func TestLedPart_OnDriveMode(t *testing.T) {
 	l := fakeLed{}
-	p := LedPart{led: &l}
+	p := LedPart{led: &l, speedZone: events.SpeedZone_FAST}
 
 	cases := []struct {
 		msg   mqtt.Message
@@ -54,6 +54,7 @@ func TestLedPart_OnDriveMode(t *testing.T) {
 		}
 	}
 }
+
 func TestLedPart_OnRecord(t *testing.T) {
 	led := fakeLed{}
 	p := LedPart{led: &led}
@@ -83,3 +84,32 @@ func TestLedPart_OnRecord(t *testing.T) {
 		}
 	}
 }
+
+func TestLedPart_OnSpeedZone(t *testing.T) {
+	l := fakeLed{}
+	p := LedPart{led: &l, driveMode: events.DriveMode_PILOT}
+
+	cases := []struct {
+		msg   mqtt.Message
+		color led.Color
+	}{
+		{testtools.NewFakeMessageFromProtobuf("speedzone", &events.SpeedZoneMessage{SpeedZone: events.SpeedZone_SLOW}), led.ColorRed},
+		{testtools.NewFakeMessageFromProtobuf("speedzone", &events.SpeedZoneMessage{SpeedZone: events.SpeedZone_NORMAL}), led.ColorYellow},
+		{testtools.NewFakeMessageFromProtobuf("speedzone", &events.SpeedZoneMessage{SpeedZone: events.SpeedZone_FAST}), led.ColorBlue},
+		{testtools.NewFakeMessageFromProtobuf("speedzone", &events.SpeedZoneMessage{SpeedZone: events.SpeedZone_UNKNOWN}), led.ColorWhite},
+	}
+
+	for _, c := range cases {
+		p.onSpeedZone(nil, c.msg)
+		time.Sleep(1 * time.Millisecond)
+		var msg events.SpeedZoneMessage
+		err := proto.Unmarshal(c.msg.Payload(), &msg)
+		if err != nil {
+			t.Errorf("unable to unmarshal drive mode message: %v", err)
+		}
+		value := msg.GetSpeedZone()
+		if l.color != c.color {
+			t.Errorf("driveMode(%v)=invalid value for color: %v, wants %v", value, l.color, c.color)
+		}
+	}
+}