diff --git a/cmd/rc-arduino/rc-arduino.go b/cmd/rc-arduino/rc-arduino.go index 461317c..e876ec3 100644 --- a/cmd/rc-arduino/rc-arduino.go +++ b/cmd/rc-arduino/rc-arduino.go @@ -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), diff --git a/doc/profil-pwm-feedback.ods b/doc/profil-pwm-feedback.ods new file mode 100644 index 0000000..d3740d7 Binary files /dev/null and b/doc/profil-pwm-feedback.ods differ diff --git a/pkg/arduino/arduino.go b/pkg/arduino/arduino.go index 6e356de..b0799b1 100644 --- a/pkg/arduino/arduino.go +++ b/pkg/arduino/arduino.go @@ -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\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P-?\d+),(?P\d+),(?P\d+),(?P\d+)`) + serialLineRegex = regexp.MustCompile(`(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P-?\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+)`) DefaultPwmThrottle = PWMConfig{ Min: MinPwmThrottle, Max: MaxPwmThrottle, @@ -30,22 +31,25 @@ var ( ) type Part struct { - client mqtt.Client - throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string - pubFrequency float64 - serial io.Reader - mutex sync.Mutex - steering, secondarySteering float32 - throttle, secondaryThrottle float32 - ctrlRecord bool - driveMode events.DriveMode - cancel chan interface{} + client mqtt.Client + 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{} useSecondaryRc bool pwmSteeringConfig *PWMConfig pwmSecondarySteeringConfig *PWMConfig pwmThrottleConfig *PWMConfig pwmSecondaryThrottleConfig *PWMConfig + + throttleFeedbackThresholds *tools.ThresholdConfig } 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, - 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 { zap.S().Panicw("unable to open serial port: %v", err) } p := &Part{ - client: client, - serial: s, - throttleTopic: throttleTopic, - steeringTopic: steeringTopic, - driveModeTopic: driveModeTopic, - switchRecordTopic: switchRecordTopic, - pubFrequency: pubFrequency, - driveMode: events.DriveMode_INVALID, - cancel: make(chan interface{}), + client: client, + serial: s, + throttleTopic: throttleTopic, + steeringTopic: steeringTopic, + driveModeTopic: driveModeTopic, + switchRecordTopic: switchRecordTopic, + throttleFeedbackTopic: throttleFeedbackTopic, + pubFrequency: pubFrequency, + driveMode: events.DriveMode_INVALID, + cancel: make(chan interface{}), pwmSteeringConfig: &DefaultPwmThrottle, 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) } diff --git a/pkg/arduino/arduino_test.go b/pkg/arduino/arduino_test.go index 7fa2f16..87f5ec2 100644 --- a/pkg/arduino/arduino_test.go +++ b/pkg/arduino/arduino_test.go @@ -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}, } @@ -227,44 +233,50 @@ func TestPublish(t *testing.T) { pubFrequency := 100. a := Part{ - client: nil, - serial: conn, - pubFrequency: pubFrequency, - throttleTopic: "car/part/arduino/throttle", - steeringTopic: "car/part/arduino/steering", - driveModeTopic: "car/part/arduino/drive_mode", - switchRecordTopic: "car/part/arduino/switch_record", - cancel: make(chan interface{}), + client: nil, + serial: conn, + pubFrequency: pubFrequency, + 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() defer a.Stop() cases := []struct { - throttle, steering float32 - driveMode events.DriveMode - switchRecord bool - expectedThrottle events.ThrottleMessage - expectedSteering events.SteeringMessage - expectedDriveMode events.DriveModeMessage - expectedSwitchRecord events.SwitchRecordMessage + 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) + } + }) + } +} diff --git a/pkg/tools/test_data/config.json b/pkg/tools/test_data/config.json new file mode 100644 index 0000000..c7b2dcf --- /dev/null +++ b/pkg/tools/test_data/config.json @@ -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 ] +} diff --git a/pkg/tools/thresholdconfig.go b/pkg/tools/thresholdconfig.go new file mode 100644 index 0000000..b232376 --- /dev/null +++ b/pkg/tools/thresholdconfig.go @@ -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. +} diff --git a/pkg/tools/thresholdconfig_test.go b/pkg/tools/thresholdconfig_test.go new file mode 100644 index 0000000..af788f7 --- /dev/null +++ b/pkg/tools/thresholdconfig_test.go @@ -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) + } + }) + } +}