feat: configure maxThrottle with radio-command

cmd/rc-throttle/rc-throttle.go

@@ -18,7 +18,8 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string
+	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, maxThrottleCtrlTopic,
+		speedZoneTopic string
 	var minThrottle, maxThrottle float64
 	var publishPilotFrequency int
 	var brakeConfig string
@@ -46,6 +47,7 @@ func main() {
 	flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic to publish throttle result, use MQTT_TOPIC_THROTTLE if args not set")
 	flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set")
 	flag.StringVar(&rcThrottleTopic, "mqtt-topic-rc-throttle", os.Getenv("MQTT_TOPIC_RC_THROTTLE"), "Mqtt topic that contains RC Throttle value, use MQTT_TOPIC_RC_THROTTLE if args not set")
+	flag.StringVar(&maxThrottleCtrlTopic, "mqtt-topic-max-throttle-ctrl", os.Getenv("MQTT_TOPIC_MAX_THROTTLE_CTRL"), "Mqtt topic where to publish max throttle value allowed, use MQTT_TOPIC_MAX_THROTTLE_CTRL if args not set")
 	flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering value, use MQTT_TOPIC_STEERING 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(&speedZoneTopic, "mqtt-topic-speed-zone", os.Getenv("MQTT_TOPIC_SPEED_ZONE"), "Mqtt topic where to subscribe speed zone events, use MQTT_TOPIC_SPEED_ZONE if args not set")
@@ -142,7 +144,7 @@ func main() {
 	}
 
 	p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-		speedZoneTopic, types.Throttle(maxThrottle), 2,
+		maxThrottleCtrlTopic, speedZoneTopic, types.Throttle(maxThrottle), 2,
 		throttle.WithThrottleProcessor(throttleProcessor),
 		throttle.WithBrakeController(brakeCtrl))
 	defer p.Stop()

pkg/throttle/controller.go

@@ -13,7 +13,7 @@ import (
 )
 
 func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-	speedZoneTopic string,
+	maxThrottleCtrlTopic, speedZoneTopic string,
 	maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
 	c := &Controller{
 		client:                client,
@@ -22,6 +22,7 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
 		rcThrottleTopic:       rcThrottleTopic,
 		steeringTopic:         steeringTopic,
 		throttleFeedbackTopic: throttleFeedbackTopic,
+		maxThrottleCtrlTopic:  maxThrottleCtrlTopic,
 		speedZoneTopic:        speedZoneTopic,
 		maxThrottle:           maxValue,
 		driveMode:             events.DriveMode_USER,
@@ -66,6 +67,7 @@ type Controller struct {
 	cancel                                                                chan interface{}
 	publishPilotFrequency                                                 int
 	driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
+	maxThrottleCtrlTopic                                                  string
 	speedZoneTopic                                                        string
 }
 
@@ -120,7 +122,7 @@ func (c *Controller) readSteering() types.Steering {
 func (c *Controller) Stop() {
 	close(c.cancel)
 	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic,
-		c.throttleFeedbackTopic, c.speedZoneTopic)
+		c.throttleFeedbackTopic, c.maxThrottleCtrlTopic, c.speedZoneTopic)
 }
 
 func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
@@ -133,6 +135,18 @@ func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
 	c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
 }
 
+func (c *Controller) onMaxThrottleCtrl(_ mqtt.Client, message mqtt.Message) {
+	var msg events.ThrottleMessage
+	err := proto.Unmarshal(message.Payload(), &msg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
+		return
+	}
+	c.muDriveMode.Lock()
+	defer c.muDriveMode.Unlock()
+	c.maxThrottle = types.Throttle(msg.GetThrottle())
+}
+
 func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
 	var msg events.DriveModeMessage
 	err := proto.Unmarshal(message.Payload(), &msg)
@@ -159,9 +173,11 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
 			return
 		}
 		zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
-		if types.Throttle(throttleMsg.GetThrottle()) > c.maxThrottle {
-			zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle)
-			throttleMsg.Throttle = float32(c.maxThrottle)
+
+		if types.Throttle(throttleMsg.GetThrottle()) > 0. {
+			maxTh := c.maxThrottle
+			current := types.Throttle(throttleMsg.GetThrottle())
+			throttleMsg.Throttle = float32(current * maxTh)
 			payloadPatched, err := proto.Marshal(&throttleMsg)
 			if err != nil {
 				zap.S().Errorf("unable to marshall throttle msg: %v", err)
@@ -217,6 +233,10 @@ var registerCallbacks = func(p *Controller) error {
 	if err != nil {
 		return err
 	}
+	err = service.RegisterCallback(p.client, p.maxThrottleCtrlTopic, p.onMaxThrottleCtrl)
+	if err != nil {
+		return err
+	}
 	err = service.RegisterCallback(p.client, p.speedZoneTopic, p.onSpeedZone)
 	if err != nil {
 		return err

pkg/throttle/controller_test.go

@@ -36,9 +36,11 @@ func TestDefaultThrottle(t *testing.T) {
 	rcThrottleTopic := "topic/rcThrottle"
 	steeringTopic := "topic/rcThrottle"
 	throttleFeedbackTopic := "topic/feedback/throttle"
+	maxThrottleCtrlTopic := "topic/max/throttle"
 	speedZoneTopic := "topic/speedZone"
 
-	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200)
+	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
+		maxThrottleCtrlTopic, speedZoneTopic, 1., 200)
 
 	cases := []*struct {
 		name             string
@@ -57,8 +59,8 @@ func TestDefaultThrottle(t *testing.T) {
 		{"test8", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}},
 		{"test9", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
 		{"test10", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}},
-		{"limit max throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
-		{"limit max throttle on copilot mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
+		{"rescale throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.24000001, Confidence: 1.0}},
+		{"rescale throttle on copilot mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.24000001, Confidence: 1.0}},
 	}
 
 	go p.Start()
@@ -121,11 +123,13 @@ func TestController_Start(t *testing.T) {
 	driveModeTopic := "topic/driveMode"
 	rcThrottleTopic := "topic/rcThrottle"
 	throttleFeedbackTopic := "topic/feedback/throttle"
+	maxThrottleCtrlTopic := "topic/max/throttle"
 	speedZoneTopic := "topic/speedZone"
 
 	type fields struct {
+		max                   types.Throttle
+		min                   types.Throttle
 		driveMode             events.DriveMode
-		min, max              types.Throttle
 		publishPilotFrequency int
 		brakeCtl              brake.Controller
 	}
@@ -134,6 +138,7 @@ func TestController_Start(t *testing.T) {
 		steering         *events.SteeringMessage
 		rcThrottle       *events.ThrottleMessage
 		throttleFeedback *events.ThrottleMessage
+		maxThrottleCtrl  *events.ThrottleMessage
 	}
 
 	tests := []struct {
@@ -146,22 +151,23 @@ func TestController_Start(t *testing.T) {
 		{
 			name: "On user drive mode, throttle from rc",
 			fields: fields{
-				driveMode:             events.DriveMode_USER,
 				max:                   0.8,
 				min:                   0.3,
+				driveMode:             events.DriveMode_USER,
 				publishPilotFrequency: publishPilotFrequency,
 				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
-				rcThrottle:       &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 		},
 		{
-			name: "On user drive mode, limit throttle to max allowed value",
+			name: "On user drive mode, rescale throttle with max allowed value",
 			fields: fields{
 				driveMode:             events.DriveMode_USER,
 				max:                   0.8,
@@ -174,8 +180,9 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
-			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+			want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
 		},
 		{
 			name: "On user drive mode, throttle can be < to  min allowed value",
@@ -191,8 +198,27 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
-			want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
+			want: &events.ThrottleMessage{Throttle: 0.080000006, Confidence: 1.0},
+		},
+		{
+			name: "On user drive mode, brake doesn't rescale throttle",
+			fields: fields{
+				driveMode:             events.DriveMode_USER,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
+			},
+			msgEvents: msgEvents{
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
 		},
 		{
 			name: "On copilot drive mode, throttle from rc",
@@ -206,11 +232,30 @@ func TestController_Start(t *testing.T) {
 			msgEvents: msgEvents{
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
-				rcThrottle:       &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 		},
+		{
+			name: "On copilot drive mode, rescale throttle with max allowed value",
+			fields: fields{
+				driveMode:             events.DriveMode_COPILOT,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
+			},
+			msgEvents: msgEvents{
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
+		},
 		{
 			name: "On copilot drive mode, limit throttle to max allowed value",
 			fields: fields{
@@ -225,8 +270,9 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
-			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+			want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
 		},
 		{
 			name: "On copilot drive mode, throttle can be < to  min allowed value",
@@ -242,8 +288,27 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
-			want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
+			want: &events.ThrottleMessage{Throttle: 0.080000006, Confidence: 1.0},
+		},
+		{
+			name: "On user drive mode, brake doesn't rescale throttle",
+			fields: fields{
+				driveMode:             events.DriveMode_COPILOT,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
+			},
+			msgEvents: msgEvents{
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
 		},
 		{
 			name: "On pilot drive mode and straight steering, use max throttle allowed",
@@ -259,6 +324,7 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
@@ -276,6 +342,7 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 		},
@@ -293,6 +360,7 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 		},
@@ -310,6 +378,7 @@ func TestController_Start(t *testing.T) {
 				steering:         &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
 				rcThrottle:       &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 				throttleFeedback: &events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0},
+				maxThrottleCtrl:  &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0},
 		},
@@ -319,7 +388,7 @@ func TestController_Start(t *testing.T) {
 		t.Run(tt.name, func(t *testing.T) {
 			c := New(nil,
 				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-				speedZoneTopic, tt.fields.max,
+				maxThrottleCtrlTopic, speedZoneTopic, tt.fields.max,
 				tt.fields.publishPilotFrequency,
 				WithThrottleProcessor(&SteeringProcessor{
 					minThrottle: tt.fields.min,
@@ -338,6 +407,7 @@ func TestController_Start(t *testing.T) {
 			c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
 			c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
 			c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
+			c.onMaxThrottleCtrl(nil, testtools.NewFakeMessageFromProtobuf(maxThrottleCtrlTopic, tt.msgEvents.maxThrottleCtrl))
 			waitPublish.Wait()
 
 			var msg events.ThrottleMessage