feat: publish pwm feedback

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

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

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),
+	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() {
 		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(time.Second / time.Duration(int(pubFrequency)) * 2)
-		time.Sleep(500 * time.Millisecond)
 
 		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)
+			}
+		})
+	}
+}

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 ]
+}

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.
+}

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