Ford: add independent C0/C1 to existing joystick mode

Add an opt-in joystick channel selector for CAN FD Fords. The steering axis directly controls C0 or C1 with the other path fields zero; keyboard keys select the channel and the existing on-screen joystick alert identifies it. Preserve standard joystick outputs and existing engagement and longitudinal behavior. Require centered input after switching, disengagement, or invalid/stale input.

Validation: 287 offline tests passed; 1,590 standard-mode frames matched the previous control outputs. Verified selected fields through the real CAN encoder at zero and moving speeds, schema compilation, and lint. Driving controller and lateral maneuver tools are unchanged.
This commit is contained in:
Isaac Barham
2026-09-14 19:30:24 -04:00
parent 16b2a3e996
commit 94a460f672
6 changed files with 324 additions and 6 deletions
+3
View File
@@ -2119,6 +2119,9 @@ struct Joystick {
# convenient for debug and live tuning
axes @0: List(Float32);
buttons @1: List(Bool);
fordChannel @2 :FordChannel;
enum FordChannel { standard @0; c0 @1; c1 @2; }
}
struct DriverStateV2 {
+14 -1
View File
@@ -175,7 +175,20 @@ def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
def ford_joystick_alert(CP, sm):
ad = sm['alertDebug']
if CP.brand == 'ford' and sm.valid['alertDebug'] and ad.alertText1 in ('Joystick Mode — C0 only', 'Joystick Mode — C1 only'):
return NormalPermanentAlert(ad.alertText1, ad.alertText2)
return None
def joystick_permanent_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
return ford_joystick_alert(CP, sm) or NormalPermanentAlert("Joystick Mode")
def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if alert := ford_joystick_alert(CP, sm):
return alert
gb = sm['carControl'].actuators.accel / 4.
steer = sm['carControl'].actuators.torque
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
@@ -220,7 +233,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
EventName.joystickDebug: {
ET.WARNING: joystick_alert,
ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
ET.PERMANENT: joystick_permanent_alert,
},
EventName.longitudinalManeuver: {
+18
View File
@@ -19,6 +19,24 @@ openpilot/tools/joystick/joystick_control.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.
### Ford C0 / C1 independently
Use the existing **Settings → Developer → Joystick Debug Mode** toggle, while offroad. On a CAN FD Ford, start the existing keyboard tool with an explicit channel:
```shell
python openpilot/tools/joystick/joystick_control.py --keyboard --ford-channel c0
```
`1` selects C0 only, `2` selects C1 only, and `0` returns to standard joystick steering. Switching zeros the steering axis. The comma screen shows **Joystick Mode — C0 only** or **C1 only**, the commanded field value, and measured wheel angle. The terminal also names the selected channel.
`A`/`D` retain their normal 5% steering-axis increments. Full scale directly commands ±5.11 metres of C0 or ±0.5 radians of C1; one increment is approximately 0.26 m or 0.025 rad. The other path fields, including C2/C3, are zero. `R` resets the axes. Gas/brake keys and joystick engagement remain unchanged. With a gamepad, use the same `--ford-channel` option without `--keyboard`.
There is no target-speed check or timed waveform. This includes zero speed if normal joystick engagement and the vehicle permit it. These are direct field commands, not desired wheel angles or the model-following controller's formulas. No MADS engagement or brake behavior is added. The existing hardware safety checks remain in force.
Start with the steering axis centered. After a channel change, disengagement, or lost/invalid input, the receiver requires a fresh centered input before applying another command. A lost joystick stream removes the Ford steering request after the existing 0.2 s timeout; a held nonzero command cannot restart on reconnection. `R` resets commands; it does not disengage joystick mode.
Without `--ford-channel`, the original keyboard/gamepad behavior remains the default. The normal lateral maneuver tools are unchanged.
### Joystick on your comma three
Plug the joystick into your comma three aux USB-C port. Then, SSH into the device and start `joystick_control.py`.
+19 -3
View File
@@ -15,7 +15,7 @@ EXPO = 0.4
class Keyboard:
def __init__(self):
def __init__(self, ford_channel='standard'):
self.kb = KBHit()
self.axis_increment = 0.05 # 5% of full actuation each key press
self.axes_map = {'w': 'gb', 's': 'gb',
@@ -23,12 +23,17 @@ class Keyboard:
self.axes_values = {'gb': 0., 'steer': 0.}
self.axes_order = ['gb', 'steer']
self.cancel = False
self.ford_channel = ford_channel
self.ford_keys = ford_channel != 'standard'
def update(self):
key = self.kb.getch().lower()
self.cancel = False
if key == 'r':
self.axes_values = dict.fromkeys(self.axes_values, 0.)
elif self.ford_keys and key in ('0', '1', '2'):
self.axes_values['steer'] = 0.
self.ford_channel = {'0': 'standard', '1': 'c0', '2': 'c1'}[key]
elif key == 'c':
self.cancel = True
elif key in self.axes_map:
@@ -41,7 +46,8 @@ class Keyboard:
class Joystick:
def __init__(self):
def __init__(self, ford_channel='standard'):
self.ford_channel = ford_channel
# This class supports a PlayStation 5 DualSense controller on the comma 3X
# TODO: find a way to get this from API or detect gamepad/PC, perhaps "inputs" doesn't support it
self.cancel_button = 'BTN_NORTH' # BTN_NORTH=X/triangle
@@ -99,10 +105,13 @@ def send_thread(joystick):
while True:
if rk.frame % 20 == 0:
print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()))
if joystick.ford_channel != 'standard':
print(f'Ford {joystick.ford_channel.upper()} only (direct field command)')
joystick_msg = messaging.new_message('testJoystick')
joystick_msg.valid = True
joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order]
joystick_msg.testJoystick.fordChannel = joystick.ford_channel
pm.send('testJoystick', joystick_msg)
@@ -126,6 +135,8 @@ if __name__ == '__main__':
'a PlayStation 5 DualSense controller on the comma 3X.',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
parser.add_argument('--ford-channel', choices=('standard', 'c0', 'c1'), default='standard',
help='CAN FD Ford only: directly control one path field; default keeps normal joystick steering')
args = parser.parse_args()
if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ:
@@ -139,9 +150,14 @@ if __name__ == '__main__':
print('Buttons')
print('- `R`: Resets axes')
print('- `C`: Cancel cruise control')
if args.ford_channel != 'standard':
print('- `1`: C0 only; `2`: C1 only; `0`: normal joystick steering (switching zeros steering)')
else:
print('Using joystick, make sure to run openpilot/cereal/messaging/bridge on your device if running over the network!')
print('If not running on a comma device, the mapping may need to be adjusted.')
joystick = Keyboard() if args.keyboard else Joystick()
if args.ford_channel != 'standard':
print('Ford direct fields: 100% = 5.11m C0 or 0.5rad C1; 5% = about 0.26m C0 or 0.025rad C1.')
print('Start centered. After disengagement or input loss, reset/center before steering again.')
joystick = Keyboard(args.ford_channel) if args.keyboard else Joystick(args.ford_channel)
joystick_control_thread(joystick)
+68 -2
View File
@@ -6,6 +6,7 @@ import numpy as np
from openpilot.cereal import messaging
from opendbc.car.structs import car
from opendbc.car.vehicle_model import VehicleModel
from opendbc.car.ford.values import FordFlags
from openpilot.common.realtime import DT_CTRL, Ratekeeper
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
@@ -13,6 +14,52 @@ from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
LongCtrlState = car.CarControl.Actuators.LongControlState
MAX_LAT_ACCEL = 3.0
FORD_FIELD_LIMITS = {1: 5.11, 2: .5} # C0 metres and C1 radians, symmetric existing command bounds
class FordJoystick:
def __init__(self, CP):
self.supported = CP.brand == 'ford' and bool(CP.flags & FordFlags.CANFD)
self.channel = 0
self.need_center = False
def update(self, CC, sm, stale):
request = sm['testJoystick']
channel = request.fordChannel.raw
if channel != self.channel:
self.need_center = True
self.channel = channel
if channel or self.need_center:
valid = (not stale and sm.valid['testJoystick'] and len(request.axes) == 2 and math.isfinite(request.axes[1])
and sm.all_checks(['carState', 'selfdriveState', 'vehicleParameters']) and sm['carState'].canValid
and math.isfinite(sm['carState'].vEgo) and math.isfinite(sm['carState'].steeringAngleDeg))
if not valid or not CC.latActive:
self.need_center = True
elif request.axes[1] == 0.:
self.need_center = False
if not valid or self.need_center or (channel and (not self.supported or channel not in FORD_FIELD_LIMITS)):
CC.latActive = False
if channel or not CC.latActive:
CC.actuators.torque = CC.actuators.steeringAngleDeg = CC.actuators.curvature = 0.
if not self.supported:
return None
# Publish the disabled path in standard mode too, clearing any previous
# custom path. card consumes carControlSP alongside carControl.
msg = messaging.new_message('carControlSP')
msg.valid = True
path = msg.carControlSP.fordLateralPath
path.enabled = channel != 0
path.valid = path.enabled and CC.latActive
if path.valid:
# Positive joystick means left; Ford's internal path coordinates are right-positive.
value = -float(np.clip(request.axes[1], -1., 1.))*FORD_FIELD_LIMITS[channel]
if channel == 1:
path.pathOffset = value
else:
path.pathAngle = value
return msg
def joystickd_thread():
@@ -20,9 +67,10 @@ def joystickd_thread():
cloudlog.info("joystickd is waiting for CarParams")
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
VM = VehicleModel(CP)
ford = FordJoystick(CP)
sm = messaging.SubMaster(['carState', 'onroadEvents', 'vehicleParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL)
pm = messaging.PubMaster(['carControl', 'controlsState'])
pm = messaging.PubMaster(['carControl', 'controlsState'] + (['carControlSP', 'alertDebug'] if ford.supported else []))
rk = Ratekeeper(100, print_delay_threshold=None)
while 1:
@@ -48,18 +96,36 @@ def joystickd_thread():
else:
joystick_axes = [0.0, 0.0]
if sm['testJoystick'].fordChannel.raw and (not sm.valid['testJoystick'] or len(joystick_axes) != 2
or not all(math.isfinite(axis) for axis in joystick_axes)):
joystick_axes = [0.0, 0.0]
should_reset_joystick = True
if CC.longActive:
actuators.accel = 4.0 * float(np.clip(joystick_axes[0], -1, 1))
actuators.longControlState = LongCtrlState.stopping if should_stop(sm['carState'].vEgo, actuators.accel) else LongCtrlState.pid
CC.cruiseControl.resume = actuators.accel > 0.0
if CC.latActive:
if CC.latActive and sm['testJoystick'].fordChannel.raw == 0:
max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['vehicleParameters'].roll))
actuators.torque = float(np.clip(joystick_axes[1], -1, 1))
actuators.steeringAngleDeg, actuators.curvature = actuators.torque * max_angle, actuators.torque * -max_curvature
ford_msg = ford.update(CC, sm, should_reset_joystick)
if ford_msg is not None:
pm.send('carControlSP', ford_msg)
alert = messaging.new_message('alertDebug')
alert.valid = ford.channel in FORD_FIELD_LIMITS
if alert.valid:
channel = f'C{ford.channel-1}'
path = ford_msg.carControlSP.fordLateralPath
command = f'{path.pathOffset:+.2f} m' if ford.channel == 1 else f'{path.pathAngle:+.4f} rad'
alert.alertDebug.alertText1 = f'Joystick Mode — {channel} only'
alert.alertDebug.alertText2 = (f'{channel}: {command} | Wheel: {sm["carState"].steeringAngleDeg:+.1f}°'
if CC.latActive else 'Inactive: engage, then reset / center steering')
pm.send('alertDebug', alert)
pm.send('carControl', cc_msg)
cs_msg = messaging.new_message('controlsState')
@@ -0,0 +1,202 @@
"""Run joystickd offline through real messages, the Ford CAN encoder, and alerts."""
from collections import defaultdict
from types import SimpleNamespace
import pytest
from opendbc.can import CANParser
from opendbc.car import Bus, structs
from opendbc.car.structs import car
from opendbc.car.ford.carcontroller import CarController
from openpilot.cereal import messaging
from openpilot.selfdrive.car.helpers import convert_carControlSP
from openpilot.selfdrive.selfdrived.events import joystick_alert, joystick_permanent_alert
from openpilot.tools.joystick import joystick_control as frontend, joystickd as daemon
def car_params(brand='ford', flags=1):
return car.CarParams.new_message(brand=brand, flags=flags, carFingerprint='FORD_F_150_LIGHTNING_MK1',
mass=3000., rotationalInertia=5500., wheelbase=3.7, centerToFront=1.7,
steerRatio=16., tireStiffnessFront=100000., tireStiffnessRear=100000.,
pcmCruise=True, openpilotLongitudinalControl=True)
def run_daemon(monkeypatch, samples, CP=None, module=daemon):
CP = car_params() if CP is None else CP
raw_cp = CP.to_bytes()
outputs, index = defaultdict(list), [-1]
class Finished(Exception):
pass
class SM(messaging.SubMaster):
def update(self, _timeout):
index[0] += 1
sample = samples[index[0]]
t = 10.+index[0]*.01
messages = []
for service in self.services:
if (service == 'vehicleParameters' and index[0] % 5) or (service == 'onroadEvents' and index[0] % 100):
continue
if service == 'testJoystick' and not sample.get('send', True):
continue
msg = messaging.new_message(service, 0) if service == 'onroadEvents' else messaging.new_message(service)
msg.valid, msg.logMonoTime = True, round(t*1e9)
if service == 'carState':
cs = msg.carState
cs.vEgo, cs.vEgoRaw = sample.get('speed', 10.), sample.get('speed', 10.)
cs.canValid = sample.get('can', True)
cs.cruiseState.enabled = True
cs.steeringAngleDeg = 2.
cs.brakePressed = sample.get('brake', False)
cs.steerFaultTemporary = sample.get('fault', False)
elif service == 'selfdriveState':
msg.selfdriveState.enabled = msg.selfdriveState.active = sample.get('active', True)
elif service == 'testJoystick':
msg.valid = sample.get('valid', True)
msg.testJoystick.axes = sample.get('axes', [sample.get('gb', .2), sample.get('steer', 0.)])
msg.testJoystick.fordChannel = sample.get('channel', 'standard')
messages.append(msg.as_reader())
self.update_msgs(t, messages)
class PM:
def __init__(self, services):
self.services = services
def send(self, service, msg):
assert service in self.services
outputs[service].append(msg.as_reader())
class RK:
def __init__(self, *args, **kwargs):
pass
def keep_time(self):
if index[0] == len(samples)-1:
raise Finished
with monkeypatch.context() as mp:
mp.setattr(module, 'Params', lambda: SimpleNamespace(get=lambda *a, **kw: raw_cp))
mp.setattr(module.messaging, 'SubMaster', SM)
mp.setattr(module.messaging, 'PubMaster', PM)
mp.setattr(module, 'Ratekeeper', RK)
with pytest.raises(Finished):
module.joystickd_thread()
return outputs
@pytest.mark.parametrize('channel', ['c0', 'c1'])
@pytest.mark.parametrize('speed', [0., 1., 6.7, 20., 35.])
@pytest.mark.parametrize('steer', [-1., -.05, .05, 1.])
def test_direct_field_reaches_real_can_at_any_speed(monkeypatch, channel, speed, steer):
samples = [{'channel': channel, 'speed': speed}]*40 + [{'channel': channel, 'speed': speed, 'steer': steer}]*10
output = run_daemon(monkeypatch, samples)
cp = structs.CarParams(flags=1, carFingerprint='FORD_F_150_LIGHTNING_MK1')
sender = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], sender.CAN.main)
vehicle = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed), acc_tja_status_stock_values=defaultdict(int),
lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
for i in range(40, 50):
cc, extra = output['carControl'][i].carControl, output['carControlSP'][i].carControlSP
assert cc.latActive and cc.longActive and cc.actuators.accel == pytest.approx(.8)
assert cc.actuators.curvature == cc.actuators.steeringAngleDeg == cc.actuators.torque == 0.
_, packets = sender.update(cc, convert_carControlSP(extra), vehicle, round((10+i*.01)*1e9))
parser.update([round((10+i*.01)*1e9), packets])
wire = parser.vl['LateralMotionControl2']
assert wire['LatCtl_D2_Rq'] == 2
assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(steer*5.11 if channel == 'c0' else 0., abs=.0051)
assert wire['LatCtlPath_An_Actl'] == pytest.approx(steer*.5 if channel == 'c1' else 0., abs=.000251)
assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
alert = output['alertDebug'][-1].alertDebug
assert alert.alertText1 == f'Joystick Mode — {channel.upper()} only'
assert 'Wheel: +2.0°' in alert.alertText2
@pytest.mark.parametrize('fault', [{'active': False}, {'fault': True}, {'can': False}, {'valid': False},
{'axes': [0.]}, {'axes': [0., float('nan')]}, {'send': False}])
def test_interruptions_zero_commands_and_require_center_before_restart(monkeypatch, fault):
base = {'channel': 'c0', 'steer': .5}
samples = [{'channel': 'c0'}]*40 + [base]*5 + [base | fault]*25 + [base]*5 + [base | {'steer': 0.}]*2 + [base]*2
output = run_daemon(monkeypatch, samples)
for i in (44, 78):
assert output['carControl'][i].carControl.latActive
assert output['carControlSP'][i].carControlSP.fordLateralPath.pathOffset != 0.
for i in (69, 74):
assert not output['carControl'][i].carControl.latActive
path = output['carControlSP'][i].carControlSP.fordLateralPath
assert path.enabled and not path.valid
assert path.pathOffset == path.pathAngle == path.curvature == path.curvatureRate == 0.
def test_switch_cannot_reinterpret_a_held_command_and_standard_clears_path(monkeypatch):
samples = ([{'channel': 'c0'}]*40 + [{'channel': 'c0', 'steer': .5}]*2
+ [{'channel': 'c1', 'steer': .5}]*2 + [{'channel': 'c1'}]*2 + [{'channel': 'c1', 'steer': .5}]*2
+ [{'channel': 'standard', 'steer': .5}]*2 + [{'channel': 'standard'}]*2 + [{'channel': 'standard', 'steer': .5}]*2)
output = run_daemon(monkeypatch, samples)
for i in (42, 43, 48, 49):
assert not output['carControl'][i].carControl.latActive
path = output['carControlSP'][46].carControlSP.fordLateralPath
assert path.pathOffset == 0. and path.pathAngle == -.25
assert output['carControl'][53].carControl.actuators.torque == .5
assert not output['carControlSP'][53].carControlSP.fordLateralPath.enabled
assert not output['alertDebug'][53].valid
@pytest.mark.parametrize('brand,flags', [('ford', 0), ('honda', 1)])
def test_explicit_ford_channel_does_not_fall_back_to_other_car_steering(monkeypatch, brand, flags):
output = run_daemon(monkeypatch, [{'channel': 'c1'}]*40+[{'channel': 'c1', 'steer': 1.}]*2, car_params(brand, flags))
cc = output['carControl'][-1].carControl
assert not cc.latActive and cc.actuators.torque == cc.actuators.curvature == 0.
assert cc.actuators.accel == pytest.approx(.8)
assert 'carControlSP' not in output
def test_keyboard_opt_in_selection_does_not_change_normal_keys(monkeypatch):
keys = iter('wa2a1d0ar')
monkeypatch.setattr(frontend, 'KBHit', lambda: SimpleNamespace(getch=lambda: next(keys)))
kb = frontend.Keyboard('c0')
kb.update()
kb.update()
assert kb.axes_values == {'gb': .05, 'steer': .05}
for channel, steer in [('c1', 0.), ('c1', .05), ('c0', 0.), ('c0', -.05), ('standard', 0.), ('standard', .05), ('standard', 0.)]:
kb.update()
assert kb.ford_channel == channel and kb.axes_values['steer'] == steer
assert kb.axes_values['gb'] == 0.
monkeypatch.setattr(frontend, 'KBHit', lambda: SimpleNamespace(getch=lambda: '2'))
assert not frontend.Keyboard().update() # opt-out keeps the previously unused key unused
def test_alert_names_selected_channel_and_preserves_standard_text():
sm = messaging.SubMaster(['carControl', 'alertDebug'])
cp, cs = car_params(), car.CarState.new_message()
cc = messaging.new_message('carControl')
cc.carControl.actuators.accel, cc.carControl.actuators.torque = 2., -.2
sm.update_msgs(10., [cc.as_reader()])
alert = joystick_alert(cp, cs, sm, False, 0, None)
assert alert.alert_text_1 == 'Joystick Mode' and alert.alert_text_2 == 'Gas: 50%, Steer: -20%'
assert joystick_permanent_alert(cp, cs, sm, False, 0, None).alert_text_2 == ''
for i, channel in enumerate(('C0', 'C1')):
msg = messaging.new_message('alertDebug')
msg.valid = True
msg.alertDebug.alertText1, msg.alertDebug.alertText2 = f'Joystick Mode — {channel} only', 'command / wheel'
sm.update_msgs(10.01+i*.01, [msg.as_reader()])
for callback in (joystick_alert, joystick_permanent_alert):
assert callback(cp, cs, sm, False, 0, None).alert_text_1 == f'Joystick Mode — {channel} only'
@pytest.mark.parametrize('channel', ['standard', 'c0', 'c1'])
def test_existing_sender_publishes_channel_with_unchanged_axes(monkeypatch, channel):
class Finished(Exception):
pass
def done():
raise Finished
captured = []
monkeypatch.setattr(frontend.messaging, 'PubMaster', lambda _services: SimpleNamespace(send=lambda _s, m: captured.append(m.as_reader())))
monkeypatch.setattr(frontend, 'Ratekeeper', lambda *a, **kw: SimpleNamespace(frame=0, keep_time=done))
joystick = SimpleNamespace(axes_values={'gb': -.2, 'steer': .35}, axes_order=['gb', 'steer'], ford_channel=channel)
with pytest.raises(Finished):
frontend.send_thread(joystick)
assert captured[0].testJoystick.fordChannel == channel
assert list(captured[0].testJoystick.axes) == pytest.approx([-.2, .35])