feat: red on brake

cmd/rc-led/rc-led.go

@@ -15,7 +15,8 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var driveModeTopic, recordTopic, speedZoneTopic string
+	var driveModeTopic, recordTopic, speedZoneTopic, throttleTopic string
+	var enableSpeedZoneMode bool
 
 	mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
 	_, mqttRetain := os.LookupEnv("MQTT_RETAIN")
@@ -25,6 +26,8 @@ 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")
+	flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic that contains throttle, use MQTT_TOPIC_THROTTLE if args not set")
+	flag.BoolVar(&enableSpeedZoneMode, "enable-speedzone-mode", false, "Enable speed-zone mode")
 
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 	flag.Parse()
@@ -53,7 +56,11 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	p := part.NewPart(client, driveModeTopic, recordTopic, speedZoneTopic)
+	mode := part.LedModeBrake
+	if enableSpeedZoneMode {
+		mode = part.LedModeSpeedZone
+	}
+	p := part.NewPart(client, driveModeTopic, recordTopic, speedZoneTopic, throttleTopic, mode)
 	defer p.Stop()
 
 	cli.HandleExit(p)

pkg/part/part.go

@@ -12,29 +12,41 @@ import (
 	"time"
 )
 
-func NewPart(client mqtt.Client, driveModeTopic, recordTopic, speedZoneTopic string) *LedPart {
+const (
+	LedModeBrake LedMode = iota
+	LedModeSpeedZone
+)
+
+type LedMode int
+
+func NewPart(client mqtt.Client, driveModeTopic, recordTopic, speedZoneTopic, throttleTopic string, ledMode LedMode) *LedPart {
 	return &LedPart{
 		led:              led.New(),
+		mode:             ledMode,
 		client:           client,
 		onDriveModeTopic: driveModeTopic,
 		onRecordTopic:    recordTopic,
 		onSpeedZoneTopic: speedZoneTopic,
+		onThrottleTopic:  throttleTopic,
 		muDriveMode:      sync.Mutex{},
 		driveMode:        events.DriveMode_INVALID,
 		muRecord:         sync.Mutex{},
 		recordEnabled:    false,
 		muSpeedZone:      sync.Mutex{},
 		speedZone:        events.SpeedZone_UNKNOWN,
+		muThrottle:       sync.Mutex{},
 	}
 
 }
 
 type LedPart struct {
 	led              led.ColoredLed
+	mode             LedMode
 	client           mqtt.Client
 	onDriveModeTopic string
 	onRecordTopic    string
 	onSpeedZoneTopic string
+	onThrottleTopic  string
 
 	muDriveMode   sync.Mutex
 	driveMode     events.DriveMode
@@ -43,6 +55,9 @@ type LedPart struct {
 
 	muSpeedZone sync.Mutex
 	speedZone   events.SpeedZone
+
+	muThrottle sync.Mutex
+	throttle   float32
 }
 
 func (p *LedPart) Start() error {
@@ -57,7 +72,7 @@ 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, p.onSpeedZoneTopic)
+	service.StopService("led", p.client, p.onDriveModeTopic, p.onRecordTopic, p.onSpeedZoneTopic, p.onThrottleTopic)
 }
 
 func (p *LedPart) setDriveMode(m events.DriveMode) {
@@ -119,12 +134,46 @@ func (p *LedPart) onSpeedZone(_ mqtt.Client, message mqtt.Message) {
 	p.updateColor()
 }
 
+func (p *LedPart) setThrottle(throttle float32) {
+	p.muThrottle.Lock()
+	defer p.muThrottle.Unlock()
+	p.throttle = throttle
+}
+
+func (p *LedPart) onThrottle(_ mqtt.Client, message mqtt.Message) {
+	var throttleMessage events.ThrottleMessage
+	err := proto.Unmarshal(message.Payload(), &throttleMessage)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal %T message: %v", throttleMessage, err)
+		return
+	}
+
+	p.setThrottle(throttleMessage.GetThrottle())
+	p.updateColor()
+}
+
 func (p *LedPart) updateColor() {
 	p.muSpeedZone.Lock()
 	defer p.muSpeedZone.Unlock()
 	p.muDriveMode.Lock()
 	defer p.muDriveMode.Unlock()
+	p.muThrottle.Lock()
+	defer p.muThrottle.Unlock()
 
+	if p.throttle <= -0.05 {
+		p.led.SetColor(led.Color{Red: int(p.throttle * -255)})
+		return
+	}
+
+	switch p.mode {
+	case LedModeBrake:
+		p.updateBrakeColor()
+	case LedModeSpeedZone:
+		p.updateSpeedZoneColor()
+	}
+}
+
+func (p *LedPart) updateSpeedZoneColor() {
 	switch p.driveMode {
 	case events.DriveMode_USER:
 		p.led.SetColor(led.ColorGreen)
@@ -142,6 +191,16 @@ func (p *LedPart) updateColor() {
 	}
 }
 
+func (p *LedPart) updateBrakeColor() {
+
+	switch p.driveMode {
+	case events.DriveMode_USER:
+		p.led.SetColor(led.ColorGreen)
+	case events.DriveMode_PILOT:
+		p.led.SetColor(led.ColorBlue)
+	}
+}
+
 func (p *LedPart) registerCallbacks() error {
 	err := service.RegisterCallback(p.client, p.onDriveModeTopic, p.onDriveMode)
 	if err != nil {
@@ -158,5 +217,10 @@ func (p *LedPart) registerCallbacks() error {
 		return err
 	}
 
+	err = service.RegisterCallback(p.client, p.onThrottleTopic, p.onThrottle)
+	if err != nil {
+		return err
+	}
+
 	return nil
 }

pkg/part/part_test.go

@@ -50,7 +50,7 @@ func TestLedPart_OnDriveMode(t *testing.T) {
 		}
 		value := msg.DriveMode
 		if l.color != c.color {
-			t.Errorf("driveMode(%v)=invalid value for color: %v, wants %v", value, l.color, c.color)
+			t.Errorf("driveMode(%v)=invalid value for expectedColor: %v, wants %v", value, l.color, c.color)
 		}
 	}
 }
@@ -87,7 +87,7 @@ func TestLedPart_OnRecord(t *testing.T) {
 
 func TestLedPart_OnSpeedZone(t *testing.T) {
 	l := fakeLed{}
-	p := LedPart{led: &l, driveMode: events.DriveMode_PILOT}
+	p := LedPart{led: &l, mode: LedModeSpeedZone, driveMode: events.DriveMode_PILOT}
 
 	cases := []struct {
 		msg   mqtt.Message
@@ -109,7 +109,65 @@ func TestLedPart_OnSpeedZone(t *testing.T) {
 		}
 		value := msg.GetSpeedZone()
 		if l.color != c.color {
-			t.Errorf("driveMode(%v)=invalid value for color: %v, wants %v", value, l.color, c.color)
+			t.Errorf("driveMode(%v)=invalid value for expectedColor: %v, wants %v", value, l.color, c.color)
 		}
 	}
 }
+
+func TestLedPart_OnThrottle(t *testing.T) {
+
+	cases := []struct {
+		name          string
+		msg           mqtt.Message
+		expectedColor led.Color
+	}{
+		{"throttle stop",
+			testtools.NewFakeMessageFromProtobuf("throttle", &events.ThrottleMessage{Throttle: 0.}),
+			led.ColorBlue,
+		},
+		{
+			"throttle normal",
+			testtools.NewFakeMessageFromProtobuf("throttle", &events.ThrottleMessage{Throttle: 0.5}),
+			led.ColorBlue,
+		},
+		{
+			"near zero",
+			testtools.NewFakeMessageFromProtobuf("throttle", &events.ThrottleMessage{Throttle: -0.01}),
+			led.ColorBlue,
+		},
+		{
+			"slow brake",
+			testtools.NewFakeMessageFromProtobuf("throttle", &events.ThrottleMessage{Throttle: -0.06}),
+			led.Color{Red: 15},
+		},
+		{
+			"normal brake",
+			testtools.NewFakeMessageFromProtobuf("throttle", &events.ThrottleMessage{Throttle: -0.5}),
+			led.Color{Red: 127},
+		},
+		{
+			"high brake",
+			testtools.NewFakeMessageFromProtobuf("throttle", &events.ThrottleMessage{Throttle: -1.}),
+			led.ColorRed,
+		},
+	}
+
+	for _, c := range cases {
+		t.Run(c.name, func(t *testing.T) {
+			l := fakeLed{}
+			p := LedPart{led: &l, mode: LedModeBrake, driveMode: events.DriveMode_PILOT}
+
+			p.onThrottle(nil, c.msg)
+			time.Sleep(1 * time.Millisecond)
+			var msg events.ThrottleMessage
+			err := proto.Unmarshal(c.msg.Payload(), &msg)
+			if err != nil {
+				t.Errorf("unable to unmarshal drive mode message: %v", err)
+			}
+			value := msg.GetThrottle()
+			if l.color != c.expectedColor {
+				t.Errorf("driveMode(%v)=invalid value for expectedColor: %v, wants %v", value, l.color, c.expectedColor)
+			}
+		})
+	}
+}