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

feat: publish pwm feedback

Browse Source
This commit is contained in:
Cyrille Nofficial
2022-09-02 11:44:14 +02:00
parent 14d5c605e7
commit e788abd5a9
7 changed files with 423 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
cmd/rc-arduino/rc-arduino.go
View File

@@ -27,7 +27,8 @@ var (
func main() { func main() {
var mqttBroker, username, password, clientId string var mqttBroker, username, password, clientId string
var throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string var throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string
var feedbackConfig string
var device string var device string
var baud int var baud int
var pubFrequency float64 var pubFrequency float64
@@ -75,8 +76,10 @@ func main() {
flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic where to publish steering values, use MQTT_TOPIC_STEERING if args not set") flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic where to publish steering values, use MQTT_TOPIC_STEERING if args not set")
flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic where to publish drive mode state, use MQTT_TOPIC_DRIVE_MODE if args not set") flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic where to publish drive mode state, use MQTT_TOPIC_DRIVE_MODE if args not set")
flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic where to publish switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set") flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic where to publish switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set")
flag.StringVar(&throttleFeedbackTopic, "mqtt-topic-throttle-feedback", os.Getenv("MQTT_TOPIC_THROTTLE_FEEDBACK"), "Mqtt topic where to publish throttle feedback, use MQTT_TOPIC_THROTTLE_FEEDBACK if args not set")
flag.StringVar(&device, "device", "/dev/serial0", "Serial device") flag.StringVar(&device, "device", "/dev/serial0", "Serial device")
flag.IntVar(&baud, "baud", 115200, "Serial baud") flag.IntVar(&baud, "baud", 115200, "Serial baud")
flag.StringVar(&feedbackConfig, "throttle-feedback-config", "", "config file that described thresholds to map pwm to percent the throttle feedback")
flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "maxPwm left value for steering PWM, STEERING_LEFT_PWM env if args not set") flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "maxPwm left value for steering PWM, STEERING_LEFT_PWM env if args not set")
flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "maxPwm right value for steering PWM, STEERING_RIGHT_PWM env if args not set") flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "maxPwm right value for steering PWM, STEERING_RIGHT_PWM env if args not set")
@@ -124,8 +127,9 @@ func main() {
tc := arduino.NewAsymetricPWMConfig(throttleMinPWM, throttleMaxPWM, throttleZeroPWM) tc := arduino.NewAsymetricPWMConfig(throttleMinPWM, throttleMaxPWM, throttleZeroPWM)
secondaryTc := arduino.NewAsymetricPWMConfig(secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleMaxPWM) secondaryTc := arduino.NewAsymetricPWMConfig(secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleMaxPWM)
a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic,
pubFrequency, pubFrequency,
arduino.WithThrottleFeedbackConfig(feedbackConfig),
arduino.WithThrottleConfig(tc), arduino.WithThrottleConfig(tc),
arduino.WithSteeringConfig(sc), arduino.WithSteeringConfig(sc),
arduino.WithSecondaryRC(secondaryTc, secondarySc), arduino.WithSecondaryRC(secondaryTc, secondarySc),

BIN
doc/profil-pwm-feedback.ods Normal file
View File

Binary file not shown.

98
pkg/arduino/arduino.go
View File

@@ -2,6 +2,7 @@ package arduino
import ( import (
"bufio" "bufio"
"github.com/cyrilix/robocar-arduino/pkg/tools"
"github.com/cyrilix/robocar-protobuf/go/events" "github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang" mqtt "github.com/eclipse/paho.mqtt.golang"
"github.com/tarm/serial" "github.com/tarm/serial"
@@ -21,7 +22,7 @@ const (
) )
var ( var (
serialLineRegex = regexp.MustCompile(`(?P<timestamp>\d+),(?P<channel_1>\d+),(?P<channel_2>\d+),(?P<channel_3>\d+),(?P<channel_4>\d+),(?P<channel_5>\d+),(?P<channel_6>-?\d+),(?P<channel_7>\d+),(?P<channel_8>\d+),(?P<frequency>\d+)`) serialLineRegex = regexp.MustCompile(`(?P<timestamp>\d+),(?P<channel_1>\d+),(?P<channel_2>\d+),(?P<channel_3>\d+),(?P<channel_4>\d+),(?P<channel_5>\d+),(?P<channel_6>-?\d+),(?P<channel_7>\d+),(?P<channel_8>\d+),(?P<channel_9>\d+),(?P<frequency>\d+)`)
DefaultPwmThrottle = PWMConfig{ DefaultPwmThrottle = PWMConfig{
Min: MinPwmThrottle, Min: MinPwmThrottle,
Max: MaxPwmThrottle, Max: MaxPwmThrottle,
@@ -30,22 +31,25 @@ var (
) )
type Part struct { type Part struct {
client mqtt.Client client mqtt.Client
throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string
pubFrequency float64 pubFrequency float64
serial io.Reader serial io.Reader
mutex sync.Mutex mutex sync.Mutex
steering, secondarySteering float32 steering, secondarySteering float32
throttle, secondaryThrottle float32 throttle, secondaryThrottle float32
ctrlRecord bool throttleFeedback float32
driveMode events.DriveMode ctrlRecord bool
cancel chan interface{} driveMode events.DriveMode
cancel chan interface{}
useSecondaryRc bool useSecondaryRc bool
pwmSteeringConfig *PWMConfig pwmSteeringConfig *PWMConfig
pwmSecondarySteeringConfig *PWMConfig pwmSecondarySteeringConfig *PWMConfig
pwmThrottleConfig *PWMConfig pwmThrottleConfig *PWMConfig
pwmSecondaryThrottleConfig *PWMConfig pwmSecondaryThrottleConfig *PWMConfig
throttleFeedbackThresholds *tools.ThresholdConfig
} }
type PWMConfig struct { type PWMConfig struct {
@@ -89,28 +93,46 @@ func WithSecondaryRC(throttleConfig, steeringConfig *PWMConfig) Option {
} }
} }
func WithThrottleFeedbackConfig(filename string) Option {
return func(p *Part) {
if filename == "" {
p.throttleFeedbackThresholds = tools.NewThresholdConfig()
return
}
tc, err := tools.NewThresholdConfigFromJson(filename)
if err != nil {
zap.S().Panicf("unable to load ThresholdConfig from file %v: %v", filename, err)
}
p.throttleFeedbackThresholds = tc
}
}
func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic, func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic,
switchRecordTopic string, pubFrequency float64, options ...Option) *Part { switchRecordTopic, throttleFeedbackTopic string, pubFrequency float64, options ...Option) *Part {
c := &serial.Config{Name: name, Baud: baud} c := &serial.Config{Name: name, Baud: baud}
s, err := serial.OpenPort(c) s, err := serial.OpenPort(c)
if err != nil { if err != nil {
zap.S().Panicw("unable to open serial port: %v", err) zap.S().Panicw("unable to open serial port: %v", err)
} }
p := &Part{ p := &Part{
client: client, client: client,
serial: s, serial: s,
throttleTopic: throttleTopic, throttleTopic: throttleTopic,
steeringTopic: steeringTopic, steeringTopic: steeringTopic,
driveModeTopic: driveModeTopic, driveModeTopic: driveModeTopic,
switchRecordTopic: switchRecordTopic, switchRecordTopic: switchRecordTopic,
pubFrequency: pubFrequency, throttleFeedbackTopic: throttleFeedbackTopic,
driveMode: events.DriveMode_INVALID, pubFrequency: pubFrequency,
cancel: make(chan interface{}), driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
pwmSteeringConfig: &DefaultPwmThrottle, pwmSteeringConfig: &DefaultPwmThrottle,
pwmSecondarySteeringConfig: &DefaultPwmThrottle, pwmSecondarySteeringConfig: &DefaultPwmThrottle,
pwmThrottleConfig: &DefaultPwmThrottle, pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle, pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
throttleFeedbackThresholds: tools.NewThresholdConfig(),
} }
for _, o := range options { for _, o := range options {
@@ -153,6 +175,7 @@ func (a *Part) updateValues(values []string) {
a.processChannel6(values[6]) a.processChannel6(values[6])
a.processChannel7(values[7]) a.processChannel7(values[7])
a.processChannel8(values[8]) a.processChannel8(values[8])
a.processChannel9(values[9])
} }
func (a *Part) Stop() { func (a *Part) Stop() {
@@ -225,6 +248,11 @@ func (a *Part) processChannel3(v string) {
func (a *Part) processChannel4(v string) { func (a *Part) processChannel4(v string) {
zap.L().Debug("process new value for channel4", zap.String("value", v)) zap.L().Debug("process new value for channel4", zap.String("value", v))
value, err := strconv.Atoi(v)
if err != nil {
zap.S().Errorf("invalid throttle value for channel2, should be an int: %v", err)
}
a.throttleFeedback = a.convertPwmFeedBackToPercent(value)
} }
func (a *Part) processChannel5(v string) { func (a *Part) processChannel5(v string) {
@@ -295,6 +323,10 @@ func (a *Part) processChannel8(v string) {
a.secondaryThrottle = ((float32(value)-float32(a.pwmSecondaryThrottleConfig.Min))/float32(a.pwmSecondaryThrottleConfig.Max-a.pwmSecondaryThrottleConfig.Min))*2.0 - 1.0 a.secondaryThrottle = ((float32(value)-float32(a.pwmSecondaryThrottleConfig.Min))/float32(a.pwmSecondaryThrottleConfig.Max-a.pwmSecondaryThrottleConfig.Min))*2.0 - 1.0
} }
func (a *Part) processChannel9(v string) {
zap.L().Debug("process new value for channel9", zap.String("value", v))
}
func (a *Part) Throttle() float32 { func (a *Part) Throttle() float32 {
a.mutex.Lock() a.mutex.Lock()
defer a.mutex.Unlock() defer a.mutex.Unlock()
@@ -304,6 +336,12 @@ func (a *Part) Throttle() float32 {
return a.throttle return a.throttle
} }
func (a *Part) ThrottleFeedback() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
return a.throttleFeedback
}
func (a *Part) Steering() float32 { func (a *Part) Steering() float32 {
a.mutex.Lock() a.mutex.Lock()
defer a.mutex.Unlock() defer a.mutex.Unlock()
@@ -341,6 +379,7 @@ func (a *Part) publishLoop() {
func (a *Part) publishValues() { func (a *Part) publishValues() {
a.publishThrottle() a.publishThrottle()
a.publishThrottleFeedback()
a.publishSteering() a.publishSteering()
a.publishDriveMode() a.publishDriveMode()
a.publishSwitchRecord() a.publishSwitchRecord()
@@ -374,6 +413,19 @@ func (a *Part) publishSteering() {
publish(a.client, a.steeringTopic, steeringMessage) publish(a.client, a.steeringTopic, steeringMessage)
} }
func (a *Part) publishThrottleFeedback() {
tm := events.ThrottleMessage{
Throttle: a.ThrottleFeedback(),
Confidence: 1.,
}
tfMessage, err := proto.Marshal(&tm)
if err != nil {
zap.S().Errorf("unable to marshal protobuf throttleFeedback message: %v", err)
return
}
publish(a.client, a.throttleFeedbackTopic, tfMessage)
}
func (a *Part) publishDriveMode() { func (a *Part) publishDriveMode() {
dm := events.DriveModeMessage{ dm := events.DriveModeMessage{
DriveMode: a.DriveMode(), DriveMode: a.DriveMode(),
@@ -398,6 +450,10 @@ func (a *Part) publishSwitchRecord() {
publish(a.client, a.switchRecordTopic, switchRecordMessage) publish(a.client, a.switchRecordTopic, switchRecordMessage)
} }
func (a *Part) convertPwmFeedBackToPercent(value int) float32 {
return float32(a.throttleFeedbackThresholds.ValueOf(value))
}
var publish = func(client mqtt.Client, topic string, payload []byte) { var publish = func(client mqtt.Client, topic string, payload []byte) {
client.Publish(topic, 0, false, payload) client.Publish(topic, 0, false, payload)
} }

187
pkg/arduino/arduino_test.go
View File

@@ -3,6 +3,7 @@ package arduino
import ( import (
"bufio" "bufio"
"fmt" "fmt"
"github.com/cyrilix/robocar-arduino/pkg/tools"
"github.com/cyrilix/robocar-protobuf/go/events" "github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang" mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto" "google.golang.org/protobuf/proto"
@@ -59,8 +60,13 @@ func TestArduinoPart_Update(t *testing.T) {
}() }()
defaultPwmThrottleConfig := NewPWMConfig(MinPwmThrottle, MaxPwmThrottle) defaultPwmThrottleConfig := NewPWMConfig(MinPwmThrottle, MaxPwmThrottle)
a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle), a := Part{client: nil, serial: conn, pubFrequency: 100,
pwmThrottleConfig: &DefaultPwmThrottle, pwmSecondaryThrottleConfig: &DefaultPwmThrottle, pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle)} pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle),
pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle),
throttleFeedbackThresholds: tools.NewThresholdConfig(),
}
go func() { go func() {
err := a.Start() err := a.Start()
if err != nil { if err != nil {
@@ -69,7 +75,7 @@ func TestArduinoPart_Update(t *testing.T) {
} }
}() }()
channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1012, 1678, 1910, 112, 0, 0 channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9 := 678, 910, 1012, 1678, 1910, 112, 0, 0, 0
cases := []struct { cases := []struct {
name, content string name, content string
throttlePwmConfig *PWMConfig throttlePwmConfig *PWMConfig
@@ -78,86 +84,86 @@ func TestArduinoPart_Update(t *testing.T) {
expectedSwitchRecord bool expectedSwitchRecord bool
}{ }{
{"Good value", {"Good value",
fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., defaultPwmThrottleConfig, -1., -1.,
events.DriveMode_USER, false}, events.DriveMode_USER, false},
{"Invalid line", {"Invalid line",
"12350,invalid line\n", defaultPwmThrottleConfig, "12350,invalid line\n", defaultPwmThrottleConfig,
-1., -1., events.DriveMode_INVALID, false}, -1., -1., events.DriveMode_INVALID, false},
{"Switch record on", {"Switch record on",
fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8), fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true},
{"Switch record off", {"Switch record off",
fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8), fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Switch record off", {"Switch record off",
fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8), fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Switch record on", {"Switch record on",
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8), fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true},
{"DriveMode: user", {"DriveMode: user",
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8), fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"DriveMode: pilot", {"DriveMode: pilot",
fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8), fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
{"DriveMode: pilot", {"DriveMode: pilot",
fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8), fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
// DriveMode: user // DriveMode: user
{"DriveMode: user", {"DriveMode: user",
fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8), fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8), {"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Sterring: left", {"Sterring: left",
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -0.92, events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -0.92, events.DriveMode_USER, false},
{"Sterring: middle", {"Sterring: middle",
fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -0.09, events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -0.09, events.DriveMode_USER, false},
{"Sterring: right", {"Sterring: right",
fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., 0.95, events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., 0.95, events.DriveMode_USER, false},
{"Sterring: over right", {"Sterring: over right",
fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., 1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., 1., events.DriveMode_USER, false},
{"Throttle: over down", {"Throttle: over down",
fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Throttle: down", {"Throttle: down",
fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, -0.95, -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -0.95, -1., events.DriveMode_USER, false},
{"Throttle: stop", {"Throttle: stop",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
NewPWMConfig(1000, 1900), 0.0, -1., events.DriveMode_USER, false}, NewPWMConfig(1000, 1900), 0.0, -1., events.DriveMode_USER, false},
{"Throttle: up", {"Throttle: up",
fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false},
{"Throttle: over up", {"Throttle: over up",
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
defaultPwmThrottleConfig, 1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, 1., -1., events.DriveMode_USER, false},
{"Throttle: zero not middle", {"Throttle: zero not middle",
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8), fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8, channel9),
&PWMConfig{1000, 1700, 1500}, &PWMConfig{1000, 1700, 1500},
0.5, -1., events.DriveMode_USER, false}, 0.5, -1., events.DriveMode_USER, false},
{"Use 2nd rc: use channels 7 and 8", {"Use 2nd rc: use channels 7 and 8",
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008), fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008, channel9),
defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false}, defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false},
{"Drive Mode: user", {"Drive Mode: user",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8), fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false},
{"Drive Mode: pilot", {"Drive Mode: pilot",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8), fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false},
{"Drive Mode: no value", {"Drive Mode: no value",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8), fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8, channel9),
defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false}, defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false},
} }
@@ -227,44 +233,50 @@ func TestPublish(t *testing.T) {
pubFrequency := 100. pubFrequency := 100.
a := Part{ a := Part{
client: nil, client: nil,
serial: conn, serial: conn,
pubFrequency: pubFrequency, pubFrequency: pubFrequency,
throttleTopic: "car/part/arduino/throttle", throttleTopic: "car/part/arduino/throttle/target",
steeringTopic: "car/part/arduino/steering", steeringTopic: "car/part/arduino/steering",
driveModeTopic: "car/part/arduino/drive_mode", driveModeTopic: "car/part/arduino/drive_mode",
switchRecordTopic: "car/part/arduino/switch_record", switchRecordTopic: "car/part/arduino/switch_record",
cancel: make(chan interface{}), throttleFeedbackTopic: "car/part/arduino/throttle/feedback",
cancel: make(chan interface{}),
} }
go a.Start() go a.Start()
defer a.Stop() defer a.Stop()
cases := []struct { cases := []struct {
throttle, steering float32 throttle, steering float32
driveMode events.DriveMode driveMode events.DriveMode
switchRecord bool throttleFeedback float32
expectedThrottle events.ThrottleMessage switchRecord bool
expectedSteering events.SteeringMessage expectedThrottle events.ThrottleMessage
expectedDriveMode events.DriveModeMessage expectedSteering events.SteeringMessage
expectedSwitchRecord events.SwitchRecordMessage expectedDriveMode events.DriveModeMessage
expectedSwitchRecord events.SwitchRecordMessage
expectedThrottleFeedback events.ThrottleMessage
}{ }{
{-1, 1, events.DriveMode_USER, true, {-1, 1, events.DriveMode_USER, 0.3, true,
events.ThrottleMessage{Throttle: -1., Confidence: 1.}, events.ThrottleMessage{Throttle: -1., Confidence: 1.},
events.SteeringMessage{Steering: 1.0, Confidence: 1.}, events.SteeringMessage{Steering: 1.0, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SwitchRecordMessage{Enabled: false}, events.SwitchRecordMessage{Enabled: false},
events.ThrottleMessage{Throttle: 0.3, Confidence: 1.},
}, },
{0, 0, events.DriveMode_PILOT, false, {0, 0, events.DriveMode_PILOT, 0.4, false,
events.ThrottleMessage{Throttle: 0., Confidence: 1.}, events.ThrottleMessage{Throttle: 0., Confidence: 1.},
events.SteeringMessage{Steering: 0., Confidence: 1.}, events.SteeringMessage{Steering: 0., Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SwitchRecordMessage{Enabled: true}, events.SwitchRecordMessage{Enabled: true},
events.ThrottleMessage{Throttle: 0.4, Confidence: 1.},
}, },
{0.87, -0.58, events.DriveMode_PILOT, false, {0.87, -0.58, events.DriveMode_PILOT, 0.5, false,
events.ThrottleMessage{Throttle: 0.87, Confidence: 1.}, events.ThrottleMessage{Throttle: 0.87, Confidence: 1.},
events.SteeringMessage{Steering: -0.58, Confidence: 1.}, events.SteeringMessage{Steering: -0.58, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SwitchRecordMessage{Enabled: true}, events.SwitchRecordMessage{Enabled: true},
events.ThrottleMessage{Throttle: 0.5, Confidence: 1.},
}, },
} }
@@ -274,17 +286,17 @@ func TestPublish(t *testing.T) {
a.steering = c.steering a.steering = c.steering
a.driveMode = c.driveMode a.driveMode = c.driveMode
a.ctrlRecord = c.switchRecord a.ctrlRecord = c.switchRecord
a.throttleFeedback = c.throttleFeedback
a.mutex.Unlock() a.mutex.Unlock()
time.Sleep(time.Second / time.Duration(int(pubFrequency))) time.Sleep(time.Second / time.Duration(int(pubFrequency)) * 2)
time.Sleep(500 * time.Millisecond)
var throttleMsg events.ThrottleMessage var throttleMsg events.ThrottleMessage
muPublishedEvents.Lock() muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle"], &throttleMsg) unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/target"], &throttleMsg)
muPublishedEvents.Unlock() muPublishedEvents.Unlock()
if throttleMsg.String() != c.expectedThrottle.String() { if throttleMsg.String() != c.expectedThrottle.String() {
t.Errorf("msg(car/part/arduino/throttle): %v, wants %v", throttleMsg, c.expectedThrottle) t.Errorf("msg(car/part/arduino/throttle/target): %v, wants %v", throttleMsg.String(), c.expectedThrottle.String())
} }
var steeringMsg events.SteeringMessage var steeringMsg events.SteeringMessage
@@ -292,7 +304,7 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/steering"], &steeringMsg) unmarshalMsg(t, pulishedEvents["car/part/arduino/steering"], &steeringMsg)
muPublishedEvents.Unlock() muPublishedEvents.Unlock()
if steeringMsg.String() != c.expectedSteering.String() { if steeringMsg.String() != c.expectedSteering.String() {
t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg, c.expectedSteering) t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg.String(), c.expectedSteering.String())
} }
var driveModeMsg events.DriveModeMessage var driveModeMsg events.DriveModeMessage
@@ -300,7 +312,7 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/drive_mode"], &driveModeMsg) unmarshalMsg(t, pulishedEvents["car/part/arduino/drive_mode"], &driveModeMsg)
muPublishedEvents.Unlock() muPublishedEvents.Unlock()
if driveModeMsg.String() != c.expectedDriveMode.String() { if driveModeMsg.String() != c.expectedDriveMode.String() {
t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg, c.expectedDriveMode) t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg.String(), c.expectedDriveMode.String())
} }
var switchRecordMsg events.SwitchRecordMessage var switchRecordMsg events.SwitchRecordMessage
@@ -308,7 +320,15 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/switch_record"], &switchRecordMsg) unmarshalMsg(t, pulishedEvents["car/part/arduino/switch_record"], &switchRecordMsg)
muPublishedEvents.Unlock() muPublishedEvents.Unlock()
if switchRecordMsg.String() != c.expectedSwitchRecord.String() { if switchRecordMsg.String() != c.expectedSwitchRecord.String() {
t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg, c.expectedSwitchRecord) t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg.String(), c.expectedSwitchRecord.String())
}
var throttleFeedbackMsg events.ThrottleMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/feedback"], &throttleFeedbackMsg)
muPublishedEvents.Unlock()
if throttleFeedbackMsg.String() != c.expectedThrottleFeedback.String() {
t.Errorf("msg(car/part/arduino/throttle/feedback): %v, wants %v", throttleFeedbackMsg.String(), c.expectedThrottleFeedback.String())
} }
} }
} }
@@ -483,3 +503,66 @@ func Test_convertPwmSteeringToPercent(t *testing.T) {
}) })
} }
} }
func TestPart_convertPwmFeedBackToPercent(t *testing.T) {
type fields struct {
}
type args struct {
value int
}
tests := []struct {
name string
fields fields
args args
want float32
}{
{
name: "big value -> 0%",
args: args{value: 1234567},
want: 0.,
},
{
name: "very slow",
args: args{10000},
want: 0.,
},
{
name: "0.07 limit",
args: args{8700},
want: 0.07,
},
{
name: "0.075",
args: args{value: 8700 - (8700-4800)/2},
want: 0.075,
},
{
name: "0.08",
args: args{value: 4800},
want: 0.08,
},
{
name: "1.0",
args: args{value: 548},
want: 1.,
},
{
name: "under lower limit",
args: args{value: 520},
want: 1.,
},
{
name: "invalid value",
args: args{value: 499},
want: 0.,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
a := &Part{throttleFeedbackThresholds: tools.NewThresholdConfig()}
if got := a.convertPwmFeedBackToPercent(tt.args.value); got != tt.want {
t.Errorf("convertPwmFeedBackToPercent() = %v, want %v", got, tt.want)
}
})
}
}

5
pkg/tools/test_data/config.json Normal file
View File

@@ -0,0 +1,5 @@
{
"threshold_steps": [ 0.07, 0.08, 0.09, 0.1, 0.125, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 ],
"min_valid": 500,
"data": [ 8700, 4800, 3500, 2550, 1850, 1387, 992, 840, 750, 700, 655, 620, 590, 570, 553, 549, 548 ]
}

65
pkg/tools/thresholdconfig.go Normal file
View File

@@ -0,0 +1,65 @@
package tools
import (
"encoding/json"
"fmt"
"os"
)
var (
defaultThresholdConfig = ThresholdConfig{
ThresholdSteps: []float64{0.07, 0.08, 0.09, 0.1, 0.125, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0},
MinValid: 500,
Data: []int{8700, 4800, 3500, 2550, 1850, 1387, 992, 840, 750, 700, 655, 620, 590, 570, 553, 549, 548},
}
)
func NewThresholdConfig() *ThresholdConfig {
return &defaultThresholdConfig
}
func NewThresholdConfigFromJson(fileName string) (*ThresholdConfig, error) {
content, err := os.ReadFile(fileName)
if err != nil {
return nil, fmt.Errorf("unable to read content from %s file: %w", fileName, err)
}
var ft ThresholdConfig
err = json.Unmarshal(content, &ft)
if err != nil {
return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err)
}
return &ft, nil
}
type ThresholdConfig struct {
ThresholdSteps []float64 `json:"threshold_steps"`
MinValid int `json:"min_valid"`
Data []int `json:"data"`
}
func (tc *ThresholdConfig) ValueOf(pwm int) float64 {
if pwm < tc.MinValid || pwm > tc.Data[0] {
return 0.
}
if pwm == tc.Data[0] {
return tc.ThresholdSteps[0]
}
if pwm < tc.Data[len(tc.Data)-1] && pwm >= tc.MinValid {
return 1.
}
// search column index
var idx int
// Start loop at 1 because first column should be skipped
for i := 1; i < len(tc.ThresholdSteps); i++ {
if pwm == tc.Data[i] {
return tc.ThresholdSteps[i]
}
if pwm > tc.Data[i] {
idx = i - 1
break
}
}
return tc.ThresholdSteps[idx] - (tc.ThresholdSteps[idx]-tc.ThresholdSteps[idx+1])/2.
}

135
pkg/tools/thresholdconfig_test.go Normal file
View File

@@ -0,0 +1,135 @@
package tools
import (
"reflect"
"testing"
)
func TestNewThresholdConfigFromJson(t *testing.T) {
type args struct {
fileName string
}
tests := []struct {
name string
args args
want *ThresholdConfig
wantErr bool
}{
{
name: "default config",
args: args{
fileName: "test_data/config.json",
},
want: &defaultThresholdConfig,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
got, err := NewThresholdConfigFromJson(tt.args.fileName)
if (err != nil) != tt.wantErr {
t.Errorf("NewThresholdConfigFromJson() error = %v, wantErr %v", err, tt.wantErr)
return
}
if !reflect.DeepEqual(*got, *tt.want) {
t.Errorf("NewThresholdConfigFromJson() got = %v, want %v", got, tt.want)
}
if !reflect.DeepEqual(got.MinValid, tt.want.MinValid) {
t.Errorf("NewThresholdConfigFromJson(), bad minValid value: got = %v, want %v", got.MinValid, tt.want.MinValid)
}
if !reflect.DeepEqual(got.ThresholdSteps, tt.want.ThresholdSteps) {
t.Errorf("NewThresholdConfigFromJson(), bad ThresholdSteps: got = %v, want %v", got.ThresholdSteps, tt.want.ThresholdSteps)
}
})
}
}
func TestThresholdConfig_ValueOf(t *testing.T) {
type fields struct {
ThresholdSteps []float64
MinValue int
Data []int
}
type args struct {
pwm int
}
tests := []struct {
name string
fields fields
args args
want float64
}{
{
name: "big value",
fields: fields{
ThresholdSteps: defaultThresholdConfig.ThresholdSteps,
MinValue: defaultThresholdConfig.MinValid,
Data: defaultThresholdConfig.Data,
},
args: args{
pwm: 11000.,
},
want: 0,
},
{
name: "little value",
fields: fields{
ThresholdSteps: defaultThresholdConfig.ThresholdSteps,
MinValue: defaultThresholdConfig.MinValid,
Data: defaultThresholdConfig.Data,
},
args: args{
pwm: defaultThresholdConfig.MinValid - 1,
},
want: 0,
},
{
name: "pwm at limit",
fields: fields{
ThresholdSteps: defaultThresholdConfig.ThresholdSteps,
MinValue: defaultThresholdConfig.MinValid,
Data: defaultThresholdConfig.Data,
},
args: args{
pwm: defaultThresholdConfig.Data[2],
},
want: defaultThresholdConfig.ThresholdSteps[2],
},
{
name: "between 2 limits",
fields: fields{
ThresholdSteps: defaultThresholdConfig.ThresholdSteps,
MinValue: defaultThresholdConfig.MinValid,
Data: defaultThresholdConfig.Data,
},
args: args{
pwm: 800,
},
want: 0.275,
},
{
name: "over last value and > minValue",
fields: fields{
ThresholdSteps: defaultThresholdConfig.ThresholdSteps,
MinValue: defaultThresholdConfig.MinValid,
Data: defaultThresholdConfig.Data,
},
args: args{
pwm: defaultThresholdConfig.MinValid + 3,
},
want: 1.,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
f := &ThresholdConfig{
ThresholdSteps: tt.fields.ThresholdSteps,
MinValid: tt.fields.MinValue,
Data: tt.fields.Data,
}
got := f.ValueOf(tt.args.pwm)
if !reflect.DeepEqual(got, tt.want) {
t.Errorf("ValueOf() = %v, want %v", got, tt.want)
}
})
}
}