Enhancements Package

+Joystick w/ Xbox One Controller Params

Author: wirelessnet2

selfdrive/car/interfaces.py

@@ -28,6 +28,8 @@ def __init__(self, CP, CarController, CarState):
     self.CP = CP
     self.VM = VehicleModel(CP)
 
+    self.hzCounter = 0
+
     self.frame = 0
     self.steering_unpressed = 0
     self.low_speed_alert = False
@@ -86,7 +88,7 @@ def get_std_params(candidate, fingerprint):
     ret.steerRatioRear = 0.  # no rear steering, at least on the listed cars aboveA
     ret.openpilotLongitudinalControl = False
     ret.stopAccel = -2.0
-    ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
+    ret.stoppingDecelRate = 0.025 # brake_travel/s while trying to stop
     ret.vEgoStopping = 0.5
     ret.vEgoStarting = 0.5  # needs to be >= vEgoStopping to avoid state transition oscillation
     ret.stoppingControl = True
@@ -152,8 +154,15 @@ def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True):
       events.add(EventName.manualSteeringRequired)
     else:
       self.silent_steer_warning = False
+
     if cs_out.steerError:
-      events.add(EventName.steerUnavailable)
+      self.hzCounter += 1
+      if self.hzCounter > 250:
+        events.add(EventName.steerUnavailable)
+        self.hzCounter = 301
+
+    if not cs_out.steerError:
+      self.hzCounter = 0
 
     # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
     if (cs_out.gasPressed and not self.CS.out.gasPressed) or \

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.8.13"
+#define COMMA_VERSION "0.8.13-Clarity"

selfdrive/controls/lib/events.py

@@ -184,7 +184,7 @@ class StartupAlert(Alert):
   def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
     super().__init__(alert_text_1, alert_text_2,
                      alert_status, AlertSize.mid,
-                     Priority.LOWER, VisualAlert.none, AudibleAlert.none, 10.),
+                     Priority.LOWER, VisualAlert.none, AudibleAlert.none, 5.),
 
 
 # ********** helper functions **********
@@ -276,12 +276,11 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof
   },
 
   EventName.startup: {
-    ET.PERMANENT: StartupAlert("Be ready to take over at any time")
+    ET.PERMANENT: StartupAlert("Be ready to take over at any time"),
   },
 
   EventName.startupMaster: {
-    ET.PERMANENT: StartupAlert("WARNING: This branch is not tested",
-                               alert_status=AlertStatus.userPrompt),
+    ET.PERMANENT: StartupAlert("Be ready to take over at any time"),
   },
 
   # Car is recognized, but marked as dashcam only

selfdrive/controls/lib/longitudinal_planner.py

@@ -17,7 +17,7 @@
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
-A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
+A_CRUISE_MAX_VALS = [1.5, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 15., 25., 40.]
 
 # Lookup table for turns

selfdrive/manager/process_config.py

@@ -34,7 +34,7 @@
   PythonProcess("thermald", "selfdrive.thermald.thermald", persistent=True),
   PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, persistent=True),
   PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True),
-  PythonProcess("updated", "selfdrive.updated", enabled=not PC, persistent=True),
+  #PythonProcess("updated", "selfdrive.updated", enabled=not PC, persistent=True), No Updates Allowed Here =P -wirelessnet2
   PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True),
   PythonProcess("statsd", "selfdrive.statsd", persistent=True),
 

selfdrive/ui/qt/widgets/prime.cc

@@ -319,7 +319,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) {
   } else {
     popup->reject();
 
-    if (prime_type) {
+    if (true) {
       mainLayout->setCurrentWidget(primeUser);
     } else {
       mainLayout->setCurrentWidget(primeAd);

tools/joystick/README.md

@@ -0,0 +1,45 @@
+# Joystick
+
+**Hardware needed**: [comma two devkit](https://comma.ai/shop/products/comma-two-devkit), laptop, joystick (optional)
+
+With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard, however a joystick is recommended for more precise control.
+
+Using a keyboard
+---
+
+To get started, ssh into your comma device and start joystickd with the following command:
+
+```shell
+tools/joystick/joystickd.py --keyboard
+```
+
+The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments.
+
+Using a joystick
+---
+
+In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` ZMQ packets over the network to the comma device. First connect a compatible joystick to your PC; joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
+
+1. Connect your laptop to your comma device's hotspot and open a new ssh shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode:
+   ```shell
+   # on your comma device
+   echo -n "1" > /data/params/d/JoystickDebugMode
+   ```
+2. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them:
+   ```shell
+   # on your comma device
+   cereal/messaging/bridge {LAPTOP_IP} testJoystick
+   ```
+3. Finally, start joystickd on your laptop and tell it to publish ZMQ packets over the network:
+   ```shell
+   # on your laptop
+   export ZMQ=1
+   tools/joystick/joystickd.py
+   ```
+
+---
+Now start your car and openpilot should go into debug mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell.
+
+Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls.
+
+![Imgur](steer.gif)

tools/joystick/joystickd.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python
+import argparse
+
+import cereal.messaging as messaging
+from common.numpy_fast import interp, clip
+from common.params import Params
+from inputs import get_gamepad
+from tools.lib.kbhit import KBHit
+
+
+class Keyboard:
+  def __init__(self):
+    self.kb = KBHit()
+    self.axis_increment = 0.05  # 5% of full actuation each key press
+    self.axes_map = {'w': 'gb', 's': 'gb',
+                     'a': 'steer', 'd': 'steer'}
+    self.axes_values = {'gb': 0., 'steer': 0.}
+
+  def update(self):
+    key = self.kb.getch().lower()
+    self.cancel = False
+    if key == 'r':
+      self.axes_values = {ax: 0. for ax in self.axes_values}
+    elif key == 'c':
+      self.cancel = True
+    elif key in self.axes_map:
+      axis = self.axes_map[key]
+      incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
+      self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1)
+    else:
+      return False
+    return True
+
+# Xbox One Controller Parameters with Halo Steering Controls -wn2
+class Joystick:
+  def __init__(self):
+    self.min_axis_value = -32767
+    self.max_axis_value = 32767
+    self.cancel_button = 'BTN_EAST'
+    self.axes_values = {'ABS_Y': 0., 'ABS_RX': 0.}  # gb, steer
+
+  def update(self):
+    joystick_event = get_gamepad()[0]
+    event = (joystick_event.code, joystick_event.state)
+    self.cancel = False
+    if event[0] == self.cancel_button and event[1] == 0:  # state 0 is falling edge
+      self.cancel = True
+    elif event[0] in self.axes_values:
+      self.max_axis_value = max(event[1], self.max_axis_value)
+      self.min_axis_value = min(event[1], self.min_axis_value)
+
+      norm = -interp(event[1], [self.min_axis_value, self.max_axis_value], [-1., 1.])
+      self.axes_values[event[0]] = norm if abs(norm) > 0.05 else 0.  # center can be noisy, deadzone of 5%
+    else:
+      return False
+    return True
+
+
+def joystick_thread(use_keyboard):
+  Params().put_bool('JoystickDebugMode', True)
+  joystick_sock = messaging.pub_sock('testJoystick')
+  joystick = Keyboard() if use_keyboard else Joystick()
+
+  while True:
+    ret = joystick.update()  # processes joystick/key events and handles state of axes
+    if ret:
+      dat = messaging.new_message('testJoystick')
+      dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_values]
+      dat.testJoystick.buttons = [joystick.cancel]
+      joystick_sock.send(dat.to_bytes())
+      print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()))
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(
+    description='Publishes events from your joystick to control your car',
+    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+  parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
+  args = parser.parse_args()
+
+  if args.keyboard:
+    print('\nGas/brake control: `W` and `S` keys\n'
+          'Steering control: `A` and `D` keys')
+    print('Buttons:\n'
+          '- `R`: Resets axes\n'
+          '- `C`: Cancel cruise control')
+  else:
+    print('\nUsing joystick, make sure to run bridge on your device if running over the network!')
+
+  joystick_thread(args.keyboard)