diff --git a/cmd/rc-arduino/rc-arduino.go b/cmd/rc-arduino/rc-arduino.go index e876ec3..5eaad72 100644 --- a/cmd/rc-arduino/rc-arduino.go +++ b/cmd/rc-arduino/rc-arduino.go @@ -27,7 +27,7 @@ var ( func main() { var mqttBroker, username, password, clientId string - var throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string + var throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic, maxThrottleCtrlTopic string var feedbackConfig string var device string var baud int @@ -39,7 +39,6 @@ func main() { cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain) var steeringLeftPWM, steeringRightPWM, steeringCenterPWM int - var secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM int if err := cli.SetIntDefaultValueFromEnv(&steeringLeftPWM, "STEERING_LEFT_PWM", SteeringLeftPWM); err != nil { zap.S().Warnf("unable to init steeringLeftPWM arg: %v", err) } @@ -51,7 +50,6 @@ func main() { } var throttleMinPWM, throttleMaxPWM, throttleZeroPWM int - var secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleCenterPWM int if err := cli.SetIntDefaultValueFromEnv(&throttleMinPWM, "THROTTLE_MIN_PWM", arduino.DefaultPwmThrottle.Min); err != nil { zap.S().Warnf("unable to init throttleMinPWM arg: %v", err) } @@ -77,6 +75,7 @@ func main() { 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(&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(&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") @@ -84,16 +83,20 @@ func main() { flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "maxPwm left value for steering PWM, STEERING_LEFT_PWM env if args not set") flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "maxPwm right value for steering PWM, STEERING_RIGHT_PWM env if args not set") flag.IntVar(&steeringCenterPWM, "steering-center-pwm", steeringCenterPWM, "middlePwm value for steering PWM, STEERING_CENTER_PWM env if args not set") - flag.IntVar(&secondarySteeringLeftPWM, "steering-secondary-left-pwm", steeringLeftPWM, "maxPwm left value for secondary radio controller steering PWM, STEERING_LEFT_PWM env if args not set") - flag.IntVar(&secondarySteeringRightPWM, "steering-secondary-right-pwm", steeringRightPWM, "maxPwm right value for secondary radio controller steering PWM, STEERING_RIGHT_PWM env if args not set") - flag.IntVar(&secondarySteeringCenterPWM, "steering-secondary-center-pwm", steeringCenterPWM, "middlePwm value for secondary radio controller steering PWM, STEERING_CENTER_PWM env if args not set") flag.IntVar(&throttleMinPWM, "throttle-min-pwm", throttleMinPWM, "maxPwm min value for throttle PWM, THROTTLE_MIN_PWM env if args not set") flag.IntVar(&throttleMaxPWM, "throttle-max-pwm", throttleMaxPWM, "maxPwm max value for throttle PWM, THROTTLE_MAX_PWM env if args not set") flag.IntVar(&throttleZeroPWM, "throttle-center-pwm", throttleZeroPWM, "middlePwm value for throttle PWM, THROTTLE_CENTER_PWM env if args not set") - flag.IntVar(&secondaryThrottleMinPWM, "throttle-secondary-min-pwm", throttleMinPWM, "maxPwm min value for secondary radio controller throttle PWM, THROTTLE_MIN_PWM env if args not set") - flag.IntVar(&secondaryThrottleMaxPWM, "throttle-secondary-max-pwm", throttleMaxPWM, "maxPwm max value for secondary radio controller throttle PWM, THROTTLE_MAX_PWM env if args not set") - flag.IntVar(&secondaryThrottleCenterPWM, "throttle-secondary-center-pwm", throttleZeroPWM, "middlePwm value for secondary radio controller throttle PWM, THROTTLE_CENTER_PWM env if args not set") + + var ctrlThrottleMinPWM, ctrlThrottleMaxPWM int + if err := cli.SetIntDefaultValueFromEnv(&ctrlThrottleMinPWM, "CTRL_THROTTLE_MIN_PWM", arduino.DefaultPwmThrottle.Min); err != nil { + zap.S().Warnf("unable to init ctlThrottleMinPWM arg: %v", err) + } + if err := cli.SetIntDefaultValueFromEnv(&ctrlThrottleMaxPWM, "CTRL_THROTTLE_MAX_PWM", arduino.DefaultPwmThrottle.Max); err != nil { + zap.S().Warnf("unable to init ctrlThrottleMaxPWM arg: %v", err) + } + flag.IntVar(&ctrlThrottleMinPWM, "ctrl-throttle-min-pwm", ctrlThrottleMinPWM, "maxPwm min value for control throttle PWM, CTRL_THROTTLE_MIN_PWM env if args not set") + flag.IntVar(&ctrlThrottleMaxPWM, "ctrl-throttle-max-pwm", ctrlThrottleMaxPWM, "maxPwm max value for control throttle PWM, CTRL_THROTTLE_MAX_PWM env if args not set") logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level") flag.Parse() @@ -123,16 +126,16 @@ func main() { defer client.Disconnect(10) sc := arduino.NewAsymetricPWMConfig(steeringLeftPWM, steeringRightPWM, steeringCenterPWM) - secondarySc := arduino.NewAsymetricPWMConfig(secondarySteeringLeftPWM, secondarySteeringRightPWM, secondarySteeringCenterPWM) + ctrlThrottlrConfig := arduino.NewPWMConfig(ctrlThrottleMinPWM, ctrlThrottleMaxPWM) tc := arduino.NewAsymetricPWMConfig(throttleMinPWM, throttleMaxPWM, throttleZeroPWM) - secondaryTc := arduino.NewAsymetricPWMConfig(secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleMaxPWM) - a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic, + a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, + throttleFeedbackTopic, maxThrottleCtrlTopic, pubFrequency, arduino.WithThrottleFeedbackConfig(feedbackConfig), arduino.WithThrottleConfig(tc), arduino.WithSteeringConfig(sc), - arduino.WithSecondaryRC(secondaryTc, secondarySc), + arduino.WithMaxThrottleCtrl(ctrlThrottlrConfig), ) cli.HandleExit(a) diff --git a/pkg/arduino/arduino.go b/pkg/arduino/arduino.go index 33add24..704c7fe 100644 --- a/pkg/arduino/arduino.go +++ b/pkg/arduino/arduino.go @@ -33,21 +33,21 @@ var ( type Part struct { client mqtt.Client throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string + maxThrottleCtrlTopic string pubFrequency float64 serial io.Reader mutex sync.Mutex - steering, secondarySteering float32 - throttle, secondaryThrottle float32 + steering float32 + throttle float32 throttleFeedback float32 + maxThrottleCtrl float32 ctrlRecord bool driveMode events.DriveMode cancel chan interface{} - useSecondaryRc bool - pwmSteeringConfig *PWMConfig - pwmSecondarySteeringConfig *PWMConfig - pwmThrottleConfig *PWMConfig - pwmSecondaryThrottleConfig *PWMConfig + pwmSteeringConfig *PWMConfig + pwmThrottleConfig *PWMConfig + pwmMaxThrottleCtrlConfig *PWMConfig throttleFeedbackThresholds *tools.ThresholdConfig } @@ -86,10 +86,9 @@ func WithSteeringConfig(steeringConfig *PWMConfig) Option { } } -func WithSecondaryRC(throttleConfig, steeringConfig *PWMConfig) Option { +func WithMaxThrottleCtrl(throttleCtrl *PWMConfig) Option { return func(p *Part) { - p.pwmSecondaryThrottleConfig = throttleConfig - p.pwmSecondarySteeringConfig = steeringConfig + p.pwmMaxThrottleCtrlConfig = throttleCtrl } } @@ -109,7 +108,7 @@ func WithThrottleFeedbackConfig(filename string) Option { } func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic, - switchRecordTopic, throttleFeedbackTopic string, pubFrequency float64, options ...Option) *Part { + switchRecordTopic, throttleFeedbackTopic, maxThrottleCtrlTopic string, pubFrequency float64, options ...Option) *Part { c := &serial.Config{Name: name, Baud: baud} s, err := serial.OpenPort(c) if err != nil { @@ -123,14 +122,14 @@ func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringT driveModeTopic: driveModeTopic, switchRecordTopic: switchRecordTopic, throttleFeedbackTopic: throttleFeedbackTopic, + maxThrottleCtrlTopic: maxThrottleCtrlTopic, pubFrequency: pubFrequency, driveMode: events.DriveMode_INVALID, cancel: make(chan interface{}), - pwmSteeringConfig: &DefaultPwmThrottle, - pwmSecondarySteeringConfig: &DefaultPwmThrottle, - pwmThrottleConfig: &DefaultPwmThrottle, - pwmSecondaryThrottleConfig: &DefaultPwmThrottle, + pwmSteeringConfig: &DefaultPwmThrottle, + pwmThrottleConfig: &DefaultPwmThrottle, + pwmMaxThrottleCtrlConfig: &DefaultPwmThrottle, throttleFeedbackThresholds: tools.NewThresholdConfig(), } @@ -195,10 +194,10 @@ func (a *Part) processChannel1(v string) { if err != nil { zap.S().Errorf("invalid steering value for channel1, should be an int: %v", err) } - a.steering = convertPwmSteeringToPercent(value, a.pwmSteeringConfig) + a.steering = convertPwmToPercent(value, a.pwmSteeringConfig) } -func convertPwmSteeringToPercent(value int, c *PWMConfig) float32 { +func convertPwmToPercent(value int, c *PWMConfig) float32 { if value < c.Min { value = c.Min } else if value > c.Max { @@ -239,11 +238,12 @@ func (a *Part) processChannel2(v string) { func (a *Part) processChannel3(v string) { zap.L().Debug("process new value for channel3", 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) + zap.S().Errorf("invalid steering value for channel1, should be an int: %v", err) } - a.useSecondaryRc = value > 1900 + a.maxThrottleCtrl = (convertPwmToPercent(value, a.pwmSteeringConfig) + 1) / 2 } func (a *Part) processChannel4(v string) { @@ -307,25 +307,10 @@ func (a *Part) processChannel6(v string) { func (a *Part) processChannel7(v string) { zap.L().Debug("process new value for secondary steering on channel7", zap.String("value", v)) - value, err := strconv.Atoi(v) - if err != nil { - zap.S().Errorf("invalid steering value for channel7, should be an int: %v", err) - } - a.secondarySteering = convertPwmSteeringToPercent(value, a.pwmSecondarySteeringConfig) } func (a *Part) processChannel8(v string) { zap.L().Debug("process new throttle value on channel8", zap.String("value", v)) - value, err := strconv.Atoi(v) - if err != nil { - zap.S().Errorf("invalid throttle value for channel8, should be an int: %v", err) - } - if value < a.pwmSecondaryThrottleConfig.Min { - value = a.pwmSecondaryThrottleConfig.Min - } else if value > a.pwmSecondaryThrottleConfig.Max { - value = a.pwmSecondaryThrottleConfig.Max - } - 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) { @@ -335,9 +320,6 @@ func (a *Part) processChannel9(v string) { func (a *Part) Throttle() float32 { a.mutex.Lock() defer a.mutex.Unlock() - if a.useSecondaryRc { - return a.secondaryThrottle - } return a.throttle } @@ -347,12 +329,15 @@ func (a *Part) ThrottleFeedback() float32 { return a.throttleFeedback } +func (a *Part) MaxThrottleCtrl() float32 { + a.mutex.Lock() + defer a.mutex.Unlock() + return a.maxThrottleCtrl +} + func (a *Part) Steering() float32 { a.mutex.Lock() defer a.mutex.Unlock() - if a.useSecondaryRc { - return a.secondarySteering - } return a.steering } @@ -388,6 +373,7 @@ func (a *Part) publishValues() { a.publishSteering() a.publishDriveMode() a.publishSwitchRecord() + a.publishMaxThrottleCtrl() } func (a *Part) publishThrottle() { @@ -431,6 +417,19 @@ func (a *Part) publishThrottleFeedback() { publish(a.client, a.throttleFeedbackTopic, tfMessage) } +func (a *Part) publishMaxThrottleCtrl() { + tm := events.ThrottleMessage{ + Throttle: a.MaxThrottleCtrl(), + Confidence: 1., + } + tfMessage, err := proto.Marshal(&tm) + if err != nil { + zap.S().Errorf("unable to marshal protobuf maxThrottleCtrl message: %v", err) + return + } + publish(a.client, a.maxThrottleCtrlTopic, tfMessage) +} + func (a *Part) publishDriveMode() { dm := events.DriveModeMessage{ DriveMode: a.DriveMode(), diff --git a/pkg/arduino/arduino_test.go b/pkg/arduino/arduino_test.go index 20fe139..a687bee 100644 --- a/pkg/arduino/arduino_test.go +++ b/pkg/arduino/arduino_test.go @@ -63,8 +63,7 @@ func TestArduinoPart_Update(t *testing.T) { a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle), pwmThrottleConfig: &DefaultPwmThrottle, - pwmSecondaryThrottleConfig: &DefaultPwmThrottle, - pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle), + pwmMaxThrottleCtrlConfig: &DefaultPwmThrottle, throttleFeedbackThresholds: tools.NewThresholdConfig(), } go func() { @@ -77,98 +76,106 @@ func TestArduinoPart_Update(t *testing.T) { 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 - expectedThrottle, expectedSteering float32 - expectedDriveMode events.DriveMode - expectedSwitchRecord bool + name, content string + throttlePwmConfig *PWMConfig + expectedThrottle, expectedSteering, expectedMaxThrottleCtrl float32 + expectedDriveMode events.DriveMode + expectedSwitchRecord bool }{ {"Good value", fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"Invalid line", "12350,invalid line\n", defaultPwmThrottleConfig, - -1., -1., events.DriveMode_INVALID, false}, + -1., -1., 0.01, events.DriveMode_INVALID, false}, {"Switch record on", fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, true}, {"Switch record off", fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"Switch record off", fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"Switch record on", fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, true}, {"DriveMode: user", fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"DriveMode: pilot", fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_PILOT, false}, {"DriveMode: pilot", fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_PILOT, false}, // DriveMode: user {"DriveMode: user", fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, // DriveMode: copilot {"DriveMode: copilot", fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1250, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_COPILOT, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_COPILOT, false}, {"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"Sterring: left", fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -0.92, events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -0.92, 0.01, events.DriveMode_USER, false}, {"Sterring: middle", fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -0.09, events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -0.09, 0.01, events.DriveMode_USER, false}, {"Sterring: right", fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., 0.95, events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., 0.95, 0.01, events.DriveMode_USER, false}, {"Sterring: over right", fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., 1., events.DriveMode_USER, false}, - + defaultPwmThrottleConfig, -1., 1., 0.01, events.DriveMode_USER, false}, {"Throttle: over down", fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"Throttle: down", fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, -0.95, -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -0.95, -1., 0.01, events.DriveMode_USER, false}, {"Throttle: stop", fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - NewPWMConfig(1000, 1900), 0.0, -1., events.DriveMode_USER, false}, + NewPWMConfig(1000, 1900), 0.0, -1., 0.01, events.DriveMode_USER, false}, {"Throttle: up", fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, 0.99, -1., 0.01, events.DriveMode_USER, false}, {"Throttle: over up", fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8, channel9), - defaultPwmThrottleConfig, 1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, 1., -1., 0.01, events.DriveMode_USER, false}, {"Throttle: zero not middle", 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,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008, channel9), - defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false}, + 0.5, -1., 0.01, events.DriveMode_USER, false}, + {"MaxThrottleCtrl: Too low value", + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, 100, channel4, channel5, channel6, 2000, 2008, channel9), + defaultPwmThrottleConfig, -1., -1, 0., events.DriveMode_USER, false}, + {"MaxThrottleCtrl: low value", + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, 1050, channel4, channel5, channel6, 2000, 2008, channel9), + defaultPwmThrottleConfig, -1., -1, 0.05, events.DriveMode_USER, false}, + {"MaxThrottleCtrl: High value", + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, 1900, channel4, channel5, channel6, 2000, 2008, channel9), + defaultPwmThrottleConfig, -1., -1, 0.91, events.DriveMode_USER, false}, + {"MaxThrottleCtrl: Too High value", + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, 4005, channel4, channel5, channel6, 2000, 2008, channel9), + defaultPwmThrottleConfig, -1., -1, 1, events.DriveMode_USER, false}, {"Drive Mode: user", fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_USER, false}, {"Drive Mode: pilot", fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_PILOT, false}, {"Drive Mode: no value", fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8, channel9), - defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false}, + defaultPwmThrottleConfig, -1., -1., 0.01, events.DriveMode_INVALID, false}, } for _, c := range cases { @@ -195,6 +202,9 @@ func TestArduinoPart_Update(t *testing.T) { if fmt.Sprintf("%0.2f", a.Steering()) != fmt.Sprintf("%0.2f", c.expectedSteering) { t.Errorf("%s: bad steering value, expected: %0.2f, actual: %.2f", c.name, c.expectedSteering, a.Steering()) } + if fmt.Sprintf("%0.2f", a.MaxThrottleCtrl()) != fmt.Sprintf("%0.2f", c.expectedMaxThrottleCtrl) { + t.Errorf("%s: bad MaxThrottleCtrl value, expected: %0.2f, actual: %.2f", c.name, c.expectedMaxThrottleCtrl, a.MaxThrottleCtrl()) + } if a.DriveMode() != c.expectedDriveMode { t.Errorf("%s: bad drive mode, expected: %v, actual:%v", c.name, c.expectedDriveMode, a.DriveMode()) } @@ -241,6 +251,7 @@ func TestPublish(t *testing.T) { serial: conn, pubFrequency: pubFrequency, throttleTopic: "car/part/arduino/throttle/target", + maxThrottleCtrlTopic: "car/part/arduino/throttle/max", steeringTopic: "car/part/arduino/steering", driveModeTopic: "car/part/arduino/drive_mode", switchRecordTopic: "car/part/arduino/switch_record", @@ -254,33 +265,38 @@ func TestPublish(t *testing.T) { throttle, steering float32 driveMode events.DriveMode throttleFeedback float32 + maxThrottleCtrl float32 switchRecord bool expectedThrottle events.ThrottleMessage expectedSteering events.SteeringMessage expectedDriveMode events.DriveModeMessage expectedSwitchRecord events.SwitchRecordMessage expectedThrottleFeedback events.ThrottleMessage + expectedMaxThrottleCtrl events.ThrottleMessage }{ - {-1, 1, events.DriveMode_USER, 0.3, true, + {-1, 1, events.DriveMode_USER, 0.3, 0.5, 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.}, + events.ThrottleMessage{Throttle: 0.5, Confidence: 1.}, }, - {0, 0, events.DriveMode_PILOT, 0.4, false, + {0, 0, events.DriveMode_PILOT, 0.4, 0.5, 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.}, + events.ThrottleMessage{Throttle: 0.5, Confidence: 1.}, }, - {0.87, -0.58, events.DriveMode_PILOT, 0.5, false, + {0.87, -0.58, events.DriveMode_PILOT, 0.5, 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.}, + events.ThrottleMessage{Throttle: 0.5, Confidence: 1.}, }, } @@ -288,6 +304,7 @@ func TestPublish(t *testing.T) { a.mutex.Lock() a.throttle = c.throttle a.steering = c.steering + a.maxThrottleCtrl = c.maxThrottleCtrl a.driveMode = c.driveMode a.ctrlRecord = c.switchRecord a.throttleFeedback = c.throttleFeedback @@ -334,6 +351,14 @@ func TestPublish(t *testing.T) { if throttleFeedbackMsg.String() != c.expectedThrottleFeedback.String() { t.Errorf("msg(car/part/arduino/throttle/feedback): %v, wants %v", throttleFeedbackMsg.String(), c.expectedThrottleFeedback.String()) } + + var maxThrottleCtrlMsg events.ThrottleMessage + muPublishedEvents.Lock() + unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/max"], &maxThrottleCtrlMsg) + muPublishedEvents.Unlock() + if maxThrottleCtrlMsg.String() != c.expectedMaxThrottleCtrl.String() { + t.Errorf("msg(car/part/arduino/throttle/max): %v, wants %v", maxThrottleCtrlMsg.String(), c.expectedMaxThrottleCtrl.String()) + } } } @@ -501,8 +526,8 @@ func Test_convertPwmSteeringToPercent(t *testing.T) { } for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - if got := convertPwmSteeringToPercent(tt.args.value, tt.args.steeringConfig); got != tt.want { - t.Errorf("convertPwmSteeringToPercent() = %v, want %v", got, tt.want) + if got := convertPwmToPercent(tt.args.value, tt.args.steeringConfig); got != tt.want { + t.Errorf("convertPwmToPercent() = %v, want %v", got, tt.want) } }) }