robocars/robocar-led
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat: red on brake

Browse Source
This commit is contained in:
Cyrille Nofficial 2023-05-10 19:52:44 +02:00
parent c971d51015
commit 8b67d8a434
3 changed files with 136 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cmd/rc-led/rc-led.go
View File

@ -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)

68
pkg/part/part.go
View File

@ -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
}

64
pkg/part/part_test.go
View File

@ -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)
}
})
}
}