feat: remove secondary controler and manage max Throttle with channel3
This commit is contained in:
parent
f960fcc768
commit
f85239537f
@ -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)
|
||||
|
||||
@ -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(),
|
||||
|
||||
@ -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)
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user