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
Show Stats 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() {
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 baud int
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(&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(&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.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(&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)
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,
arduino.WithThrottleFeedbackConfig(feedbackConfig),
arduino.WithThrottleConfig(tc),
arduino.WithSteeringConfig(sc),
arduino.WithSecondaryRC(secondaryTc, secondarySc),

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

Binary file not shown.

62
pkg/arduino/arduino.go
View File

@ -2,6 +2,7 @@ package arduino
import (
"bufio"
"github.com/cyrilix/robocar-arduino/pkg/tools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"github.com/tarm/serial"
@ -21,7 +22,7 @@ const (
)
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{
Min: MinPwmThrottle,
Max: MaxPwmThrottle,
@ -31,12 +32,13 @@ var (
type Part struct {
client mqtt.Client
throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string
throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string
pubFrequency float64
serial io.Reader
mutex sync.Mutex
steering, secondarySteering float32
throttle, secondaryThrottle float32
throttleFeedback float32
ctrlRecord bool
driveMode events.DriveMode
cancel chan interface{}
@ -46,6 +48,8 @@ type Part struct {
pwmSecondarySteeringConfig *PWMConfig
pwmThrottleConfig *PWMConfig
pwmSecondaryThrottleConfig *PWMConfig
throttleFeedbackThresholds *tools.ThresholdConfig
}
type PWMConfig struct {
@ -89,8 +93,23 @@ 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,
switchRecordTopic string, pubFrequency float64, options ...Option) *Part {
switchRecordTopic, throttleFeedbackTopic string, pubFrequency float64, options ...Option) *Part {
c := &serial.Config{Name: name, Baud: baud}
s, err := serial.OpenPort(c)
if err != nil {
@ -103,6 +122,7 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
steeringTopic: steeringTopic,
driveModeTopic: driveModeTopic,
switchRecordTopic: switchRecordTopic,
throttleFeedbackTopic: throttleFeedbackTopic,
pubFrequency: pubFrequency,
driveMode: events.DriveMode_INVALID,
cancel: make(chan interface{}),
@ -111,6 +131,8 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT
pwmSecondarySteeringConfig: &DefaultPwmThrottle,
pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
throttleFeedbackThresholds: tools.NewThresholdConfig(),
}
for _, o := range options {
@ -153,6 +175,7 @@ func (a *Part) updateValues(values []string) {
a.processChannel6(values[6])
a.processChannel7(values[7])
a.processChannel8(values[8])
a.processChannel9(values[9])
}
func (a *Part) Stop() {
@ -225,6 +248,11 @@ func (a *Part) processChannel3(v string) {
func (a *Part) processChannel4(v string) {
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) {
@ -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
}
func (a *Part) processChannel9(v string) {
zap.L().Debug("process new value for channel9", zap.String("value", v))
}
func (a *Part) Throttle() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
@ -304,6 +336,12 @@ func (a *Part) Throttle() float32 {
return a.throttle
}
func (a *Part) ThrottleFeedback() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
return a.throttleFeedback
}
func (a *Part) Steering() float32 {
a.mutex.Lock()
defer a.mutex.Unlock()
@ -341,6 +379,7 @@ func (a *Part) publishLoop() {
func (a *Part) publishValues() {
a.publishThrottle()
a.publishThrottleFeedback()
a.publishSteering()
a.publishDriveMode()
a.publishSwitchRecord()
@ -374,6 +413,19 @@ func (a *Part) publishSteering() {
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() {
dm := events.DriveModeMessage{
DriveMode: a.DriveMode(),
@ -398,6 +450,10 @@ func (a *Part) publishSwitchRecord() {
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) {
client.Publish(topic, 0, false, payload)
}

159
pkg/arduino/arduino_test.go
View File

@ -3,6 +3,7 @@ package arduino
import (
"bufio"
"fmt"
"github.com/cyrilix/robocar-arduino/pkg/tools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
@ -59,8 +60,13 @@ func TestArduinoPart_Update(t *testing.T) {
}()
defaultPwmThrottleConfig := NewPWMConfig(MinPwmThrottle, MaxPwmThrottle)
a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle),
pwmThrottleConfig: &DefaultPwmThrottle, pwmSecondaryThrottleConfig: &DefaultPwmThrottle, pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle)}
a := Part{client: nil, serial: conn, pubFrequency: 100,
pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle),
pwmThrottleConfig: &DefaultPwmThrottle,
pwmSecondaryThrottleConfig: &DefaultPwmThrottle,
pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle),
throttleFeedbackThresholds: tools.NewThresholdConfig(),
}
go func() {
err := a.Start()
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 {
name, content string
throttlePwmConfig *PWMConfig
@ -78,86 +84,86 @@ func TestArduinoPart_Update(t *testing.T) {
expectedSwitchRecord bool
}{
{"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.,
events.DriveMode_USER, false},
{"Invalid line",
"12350,invalid line\n", defaultPwmThrottleConfig,
-1., -1., events.DriveMode_INVALID, false},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
// 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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
{"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},
0.5, -1., events.DriveMode_USER, false},
{"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},
{"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},
{"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},
{"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},
}
@ -230,10 +236,11 @@ func TestPublish(t *testing.T) {
client: nil,
serial: conn,
pubFrequency: pubFrequency,
throttleTopic: "car/part/arduino/throttle",
throttleTopic: "car/part/arduino/throttle/target",
steeringTopic: "car/part/arduino/steering",
driveModeTopic: "car/part/arduino/drive_mode",
switchRecordTopic: "car/part/arduino/switch_record",
throttleFeedbackTopic: "car/part/arduino/throttle/feedback",
cancel: make(chan interface{}),
}
go a.Start()
@ -242,29 +249,34 @@ func TestPublish(t *testing.T) {
cases := []struct {
throttle, steering float32
driveMode events.DriveMode
throttleFeedback float32
switchRecord bool
expectedThrottle events.ThrottleMessage
expectedSteering events.SteeringMessage
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.SteeringMessage{Steering: 1.0, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
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.SteeringMessage{Steering: 0., Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
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.SteeringMessage{Steering: -0.58, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
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.driveMode = c.driveMode
a.ctrlRecord = c.switchRecord
a.throttleFeedback = c.throttleFeedback
a.mutex.Unlock()
time.Sleep(time.Second / time.Duration(int(pubFrequency)))
time.Sleep(500 * time.Millisecond)
time.Sleep(time.Second / time.Duration(int(pubFrequency)) * 2)
var throttleMsg events.ThrottleMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle"], &throttleMsg)
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/target"], &throttleMsg)
muPublishedEvents.Unlock()
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
@ -292,7 +304,7 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/steering"], &steeringMsg)
muPublishedEvents.Unlock()
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
@ -300,7 +312,7 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/drive_mode"], &driveModeMsg)
muPublishedEvents.Unlock()
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
@ -308,7 +320,15 @@ func TestPublish(t *testing.T) {
unmarshalMsg(t, pulishedEvents["car/part/arduino/switch_record"], &switchRecordMsg)
muPublishedEvents.Unlock()
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)
}
})
}
}