diff --git a/cmd/rc-led/rc-led.go b/cmd/rc-led/rc-led.go index 8df25d6..b6b1e75 100644 --- a/cmd/rc-led/rc-led.go +++ b/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) diff --git a/pkg/part/part.go b/pkg/part/part.go index a436818..4d76d30 100644 --- a/pkg/part/part.go +++ b/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 } diff --git a/pkg/part/part_test.go b/pkg/part/part_test.go index d093711..eaf5774 100644 --- a/pkg/part/part_test.go +++ b/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) + } + }) + } +}