wip

tools/sim/lib/camerad.py

@@ -0,0 +1,70 @@
+import numpy as np
+import os
+import pyopencl as cl
+import pyopencl.array as cl_array
+
+from cereal.visionipc import VisionIpcServer, VisionStreamType
+from cereal import messaging
+
+from openpilot.common.basedir import BASEDIR
+from openpilot.tools.sim.lib.common import W, H
+
+class Camerad:
+  """Simulates the camerad daemon"""
+  def __init__(self, dual_camera):
+    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState'])
+
+    self.frame_road_id = 0
+    self.frame_wide_id = 0
+    self.vipc_server = VisionIpcServer("camerad")
+
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 5, False, W, H)
+    if dual_camera:
+      self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 5, False, W, H)
+
+    self.vipc_server.start_listener()
+
+    # set up for pyopencl rgb to yuv conversion
+    self.ctx = cl.create_some_context()
+    self.queue = cl.CommandQueue(self.ctx)
+    cl_arg = f" -DHEIGHT={H} -DWIDTH={W} -DRGB_STRIDE={W * 3} -DUV_WIDTH={W // 2} -DUV_HEIGHT={H // 2} -DRGB_SIZE={W * H} -DCL_DEBUG "
+
+    kernel_fn = os.path.join(BASEDIR, "tools/sim/rgb_to_nv12.cl")
+    with open(kernel_fn) as f:
+      prg = cl.Program(self.ctx, f.read()).build(cl_arg)
+      self.krnl = prg.rgb_to_nv12
+    self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4
+    self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4
+
+  def cam_send_yuv_road(self, yuv):
+    self._send_yuv(yuv, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD)
+    self.frame_road_id += 1
+
+  def cam_send_yuv_wide_road(self, yuv):
+    self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD)
+    self.frame_wide_id += 1
+
+  # Returns: yuv bytes
+  def rgb_to_yuv(self, rgb):
+    assert rgb.shape == (H, W, 3), f"{rgb.shape}"
+    assert rgb.dtype == np.uint8
+
+    rgb_cl = cl_array.to_device(self.queue, rgb)
+    yuv_cl = cl_array.empty_like(rgb_cl)
+    self.krnl(self.queue, (self.Wdiv4, self.Hdiv4), None, rgb_cl.data, yuv_cl.data).wait()
+    yuv = np.resize(yuv_cl.get(), rgb.size // 2)
+    return yuv.data.tobytes()
+
+  def _send_yuv(self, yuv, frame_id, pub_type, yuv_type):
+    eof = int(frame_id * 0.05 * 1e9)
+    self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof)
+
+    dat = messaging.new_message(pub_type)
+    msg = {
+      "frameId": frame_id,
+      "transform": [1.0, 0.0, 0.0,
+                    0.0, 1.0, 0.0,
+                    0.0, 0.0, 1.0]
+    }
+    setattr(dat, pub_type, msg)
+    self.pm.send(pub_type, dat)

tools/sim/lib/common.py

@@ -0,0 +1,94 @@
+import math
+import threading
+import numpy as np
+
+from abc import ABC, abstractmethod
+from collections import namedtuple
+
+W, H = 1928, 1208
+
+
+vec3 = namedtuple("vec3", ["x", "y", "z"])
+
+class GPSState:
+  def __init__(self):
+    self.latitude = 0
+    self.longitude = 0
+    self.altitude = 0
+
+  def from_xy(self, xy):
+    """Simulates a lat/lon from an xy coordinate on a plane, for simple simlation. TODO: proper global projection?"""
+    BASE_LAT = 32.75308505188913
+    BASE_LON = -117.2095393365393
+    DEG_TO_METERS = 100000
+
+    self.latitude = float(BASE_LAT + xy[0] / DEG_TO_METERS)
+    self.longitude = float(BASE_LON + xy[1] / DEG_TO_METERS)
+    self.altitude = 0
+
+
+class IMUState:
+  def __init__(self):
+    self.accelerometer: vec3 = vec3(0,0,0)
+    self.gyroscope: vec3 = vec3(0,0,0)
+    self.bearing: float = 0
+
+
+class SimulatorState:
+  def __init__(self):
+    self.valid = False
+    self.is_engaged = False
+    self.ignition = True
+
+    self.velocity: vec3 = None
+    self.bearing: float = 0
+    self.gps = GPSState()
+    self.imu = IMUState()
+
+    self.steering_angle: float = 0
+
+    self.user_gas: float = 0
+    self.user_brake: float = 0
+    self.user_torque: float = 0
+
+    self.cruise_button = 0
+
+    self.left_blinker = False
+    self.right_blinker = False
+
+  @property
+  def speed(self):
+    return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
+
+
+class World(ABC):
+  def __init__(self, dual_camera):
+    self.dual_camera = dual_camera
+
+    self.image_lock = threading.Lock()
+    self.road_image = np.zeros((H, W, 3), dtype=np.uint8)
+    self.wide_road_image = np.zeros((H, W, 3), dtype=np.uint8)
+
+  @abstractmethod
+  def apply_controls(self, steer_sim, throttle_out, brake_out):
+    pass
+
+  @abstractmethod
+  def tick(self):
+    pass
+
+  @abstractmethod
+  def read_sensors(self, simulator_state: SimulatorState):
+    pass
+
+  @abstractmethod
+  def read_cameras(self):
+    pass
+
+  @abstractmethod
+  def close(self):
+    pass
+
+  @abstractmethod
+  def reset(self):
+    pass

tools/sim/lib/keyboard_ctrl.py

@@ -0,0 +1,94 @@
+import sys
+import termios
+import time
+
+from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
+                     ISTRIP, IXON, PARENB, VMIN, VTIME)
+from typing import NoReturn
+
+# Indexes for termios list.
+IFLAG = 0
+OFLAG = 1
+CFLAG = 2
+LFLAG = 3
+ISPEED = 4
+OSPEED = 5
+CC = 6
+
+STDIN_FD = sys.stdin.fileno()
+
+
+KEYBOARD_HELP = """
+  | key  |   functionality       |
+  |------|-----------------------|
+  |  1   | Cruise Resume / Accel |
+  |  2   | Cruise Set    / Decel |
+  |  3   | Cruise Cancel         |
+  |  r   | Reset Simulation      |
+  |  i   | Toggle Ignition       |
+  |  q   | Exit all              |
+  | wasd | Control manually      |
+"""
+
+
+def getch() -> str:
+  old_settings = termios.tcgetattr(STDIN_FD)
+  try:
+    # set
+    mode = old_settings.copy()
+    mode[IFLAG] &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON)
+    #mode[OFLAG] &= ~(OPOST)
+    mode[CFLAG] &= ~(CSIZE | PARENB)
+    mode[CFLAG] |= CS8
+    mode[LFLAG] &= ~(ECHO | ICANON | IEXTEN)
+    mode[CC][VMIN] = 1
+    mode[CC][VTIME] = 0
+    termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, mode)
+
+    ch = sys.stdin.read(1)
+  finally:
+    termios.tcsetattr(STDIN_FD, termios.TCSADRAIN, old_settings)
+  return ch
+
+def keyboard_poll_thread(q: 'Queue[str]'):
+  while True:
+    c = getch()
+    if c == '1':
+      q.put("cruise_up")
+    elif c == '2':
+      q.put("cruise_down")
+    elif c == '3':
+      q.put("cruise_cancel")
+    elif c == 'w':
+      q.put("throttle_%f" % 1.0)
+    elif c == 'a':
+      q.put("steer_%f" % 0.15)
+    elif c == 's':
+      q.put("brake_%f" % 1.0)
+    elif c == 'd':
+      q.put("steer_%f" % -0.15)
+    elif c == 'z':
+      q.put("blinker_left")
+    elif c == 'x':
+      q.put("blinker_right")
+    elif c == 'i':
+      q.put("ignition")
+    elif c == 'r':
+      q.put("reset")
+    elif c == 'q':
+      q.put("quit")
+      break
+
+def test(q: 'Queue[str]') -> NoReturn:
+  while True:
+    print([q.get_nowait() for _ in range(q.qsize())] or None)
+    time.sleep(0.25)
+
+if __name__ == '__main__':
+  from multiprocessing import Process, Queue
+  q: Queue[str] = Queue()
+  p = Process(target=test, args=(q,))
+  p.daemon = True
+  p.start()
+
+  keyboard_poll_thread(q)

tools/sim/lib/manual_ctrl.py

@@ -0,0 +1,188 @@
+#!/usr/bin/env python3
+# set up wheel
+import array
+import os
+import struct
+from fcntl import ioctl
+from typing import NoReturn, Dict, List
+
+# Iterate over the joystick devices.
+print('Available devices:')
+for fn in os.listdir('/dev/input'):
+  if fn.startswith('js'):
+    print(f'  /dev/input/{fn}')
+
+# We'll store the states here.
+axis_states: Dict[str, float] = {}
+button_states: Dict[str, float] = {}
+
+# These constants were borrowed from linux/input.h
+axis_names = {
+  0x00 : 'x',
+  0x01 : 'y',
+  0x02 : 'z',
+  0x03 : 'rx',
+  0x04 : 'ry',
+  0x05 : 'rz',
+  0x06 : 'trottle',
+  0x07 : 'rudder',
+  0x08 : 'wheel',
+  0x09 : 'gas',
+  0x0a : 'brake',
+  0x10 : 'hat0x',
+  0x11 : 'hat0y',
+  0x12 : 'hat1x',
+  0x13 : 'hat1y',
+  0x14 : 'hat2x',
+  0x15 : 'hat2y',
+  0x16 : 'hat3x',
+  0x17 : 'hat3y',
+  0x18 : 'pressure',
+  0x19 : 'distance',
+  0x1a : 'tilt_x',
+  0x1b : 'tilt_y',
+  0x1c : 'tool_width',
+  0x20 : 'volume',
+  0x28 : 'misc',
+}
+
+button_names = {
+  0x120 : 'trigger',
+  0x121 : 'thumb',
+  0x122 : 'thumb2',
+  0x123 : 'top',
+  0x124 : 'top2',
+  0x125 : 'pinkie',
+  0x126 : 'base',
+  0x127 : 'base2',
+  0x128 : 'base3',
+  0x129 : 'base4',
+  0x12a : 'base5',
+  0x12b : 'base6',
+  0x12f : 'dead',
+  0x130 : 'a',
+  0x131 : 'b',
+  0x132 : 'c',
+  0x133 : 'x',
+  0x134 : 'y',
+  0x135 : 'z',
+  0x136 : 'tl',
+  0x137 : 'tr',
+  0x138 : 'tl2',
+  0x139 : 'tr2',
+  0x13a : 'select',
+  0x13b : 'start',
+  0x13c : 'mode',
+  0x13d : 'thumbl',
+  0x13e : 'thumbr',
+
+  0x220 : 'dpad_up',
+  0x221 : 'dpad_down',
+  0x222 : 'dpad_left',
+  0x223 : 'dpad_right',
+
+  # XBox 360 controller uses these codes.
+  0x2c0 : 'dpad_left',
+  0x2c1 : 'dpad_right',
+  0x2c2 : 'dpad_up',
+  0x2c3 : 'dpad_down',
+}
+
+axis_name_list: List[str] = []
+button_name_list: List[str] = []
+
+def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
+  # Open the joystick device.
+  fn = '/dev/input/js0'
+  print(f'Opening {fn}...')
+  jsdev = open(fn, 'rb')
+
+  # Get the device name.
+  #buf = bytearray(63)
+  buf = array.array('B', [0] * 64)
+  ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf)  # JSIOCGNAME(len)
+  js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
+  print(f'Device name: {js_name}')
+
+  # Get number of axes and buttons.
+  buf = array.array('B', [0])
+  ioctl(jsdev, 0x80016a11, buf)  # JSIOCGAXES
+  num_axes = buf[0]
+
+  buf = array.array('B', [0])
+  ioctl(jsdev, 0x80016a12, buf)  # JSIOCGBUTTONS
+  num_buttons = buf[0]
+
+  # Get the axis map.
+  buf = array.array('B', [0] * 0x40)
+  ioctl(jsdev, 0x80406a32, buf)  # JSIOCGAXMAP
+
+  for _axis in buf[:num_axes]:
+    axis_name = axis_names.get(_axis, f'unknown(0x{_axis:02x})')
+    axis_name_list.append(axis_name)
+    axis_states[axis_name] = 0.0
+
+  # Get the button map.
+  buf = array.array('H', [0] * 200)
+  ioctl(jsdev, 0x80406a34, buf)  # JSIOCGBTNMAP
+
+  for btn in buf[:num_buttons]:
+    btn_name = button_names.get(btn, f'unknown(0x{btn:03x})')
+    button_name_list.append(btn_name)
+    button_states[btn_name] = 0
+
+  print('%d axes found: %s' % (num_axes, ', '.join(axis_name_list)))
+  print('%d buttons found: %s' % (num_buttons, ', '.join(button_name_list)))
+
+  # Enable FF
+  import evdev
+  from evdev import ecodes, InputDevice
+  device = evdev.list_devices()[0]
+  evtdev = InputDevice(device)
+  val = 24000
+  evtdev.write(ecodes.EV_FF, ecodes.FF_AUTOCENTER, val)
+
+  while True:
+    evbuf = jsdev.read(8)
+    value, mtype, number = struct.unpack('4xhBB', evbuf)
+    # print(mtype, number, value)
+    if mtype & 0x02:  # wheel & paddles
+      axis = axis_name_list[number]
+
+      if axis == "z":  # gas
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = (1 - fvalue) * 50
+        q.put(f"throttle_{normalized:f}")
+
+      elif axis == "rz":  # brake
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = (1 - fvalue) * 50
+        q.put(f"brake_{normalized:f}")
+
+      elif axis == "x":  # steer angle
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = fvalue
+        q.put(f"steer_{normalized:f}")
+
+    elif mtype & 0x01:  # buttons
+      if value == 1: # press down
+        if number in [0, 19]:  # X
+          q.put("cruise_down")
+
+        elif number in [3, 18]:  # triangle
+          q.put("cruise_up")
+
+        elif number in [1, 6]:  # square
+          q.put("cruise_cancel")
+
+        elif number in [10, 21]:  # R3
+          q.put("reverse_switch")
+
+if __name__ == '__main__':
+  from multiprocessing import Process, Queue
+  q: Queue[str] = Queue()
+  p = Process(target=wheel_poll_thread, args=(q,))
+  p.start()

tools/sim/lib/simulated_car.py

@@ -0,0 +1,119 @@
+import cereal.messaging as messaging
+
+from opendbc.can.packer import CANPacker
+from opendbc.can.parser import CANParser
+from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
+from openpilot.selfdrive.car import crc8_pedal
+from openpilot.tools.sim.lib.common import SimulatorState
+
+
+class SimulatedCar:
+  """Simulates a honda civic 2016 (panda state + can messages) to OpenPilot"""
+  packer = CANPacker("honda_civic_touring_2016_can_generated")
+  rpacker = CANPacker("acura_ilx_2016_nidec")
+
+  def __init__(self):
+    self.pm = messaging.PubMaster(['can', 'pandaStates'])
+    self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams'])
+    self.cp = self.get_car_can_parser()
+    self.idx = 0
+
+  @staticmethod
+  def get_car_can_parser():
+    dbc_f = 'honda_civic_touring_2016_can_generated'
+    checks = [
+      (0xe4, 100),
+      (0x1fa, 50),
+      (0x200, 50),
+    ]
+    return CANParser(dbc_f, checks, 0)
+
+  def send_can_messages(self, simulator_state: SimulatorState):
+    if not simulator_state.valid:
+      return
+
+    msg = []
+
+    # *** powertrain bus ***
+
+    speed = simulator_state.speed * 3.6 # convert m/s to kph
+    msg.append(self.packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}))
+    msg.append(self.packer.make_can_msg("WHEEL_SPEEDS", 0, {
+      "WHEEL_SPEED_FL": speed,
+      "WHEEL_SPEED_FR": speed,
+      "WHEEL_SPEED_RL": speed,
+      "WHEEL_SPEED_RR": speed
+    }))
+
+    msg.append(self.packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": simulator_state.cruise_button}))
+
+    values = {
+      "COUNTER_PEDAL": self.idx & 0xF,
+      "INTERCEPTOR_GAS": simulator_state.user_gas * 2**12,
+      "INTERCEPTOR_GAS2": simulator_state.user_gas * 2**12,
+    }
+    checksum = crc8_pedal(self.packer.make_can_msg("GAS_SENSOR", 0, values)[2][:-1])
+    values["CHECKSUM_PEDAL"] = checksum
+    msg.append(self.packer.make_can_msg("GAS_SENSOR", 0, values))
+
+    msg.append(self.packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8}))
+    msg.append(self.packer.make_can_msg("GAS_PEDAL_2", 0, {}))
+    msg.append(self.packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}))
+    msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {"STEER_TORQUE_SENSOR": simulator_state.user_torque}))
+    msg.append(self.packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": simulator_state.steering_angle}))
+    msg.append(self.packer.make_can_msg("VSA_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if simulator_state.speed >= 1.0 else 0}))
+    msg.append(self.packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {}))
+    msg.append(self.packer.make_can_msg("EPB_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("DOORS_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("CRUISE_PARAMS", 0, {}))
+    msg.append(self.packer.make_can_msg("CRUISE", 0, {}))
+    msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0,
+                                    {
+                                      "MAIN_ON": 1,
+                                      "LEFT_BLINKER": simulator_state.left_blinker,
+                                      "RIGHT_BLINKER": simulator_state.right_blinker
+                                    }))
+    msg.append(self.packer.make_can_msg("POWERTRAIN_DATA", 0,
+                                    {
+                                    "ACC_STATUS": int(simulator_state.is_engaged),
+                                    "PEDAL_GAS": simulator_state.user_gas,
+                                    "BRAKE_PRESSED": simulator_state.user_brake > 0
+                                    }))
+    msg.append(self.packer.make_can_msg("HUD_SETTING", 0, {}))
+    msg.append(self.packer.make_can_msg("CAR_SPEED", 0, {}))
+
+    # *** cam bus ***
+    msg.append(self.packer.make_can_msg("STEERING_CONTROL", 2, {}))
+    msg.append(self.packer.make_can_msg("ACC_HUD", 2, {}))
+    msg.append(self.packer.make_can_msg("LKAS_HUD", 2, {}))
+    msg.append(self.packer.make_can_msg("BRAKE_COMMAND", 2, {}))
+
+    # *** radar bus ***
+    if self.idx % 5 == 0:
+      msg.append(self.rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79}))
+      for i in range(16):
+        msg.append(self.rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5}))
+
+    self.pm.send('can', can_list_to_can_capnp(msg))
+
+  def send_panda_state(self, simulator_state):
+    self.sm.update(0)
+    dat = messaging.new_message('pandaStates', 1)
+    dat.valid = True
+    dat.pandaStates[0] = {
+      'ignitionLine': simulator_state.ignition,
+      'pandaType': "blackPanda",
+      'controlsAllowed': True,
+      'safetyModel': 'hondaNidec',
+      'alternativeExperience': self.sm["carParams"].alternativeExperience
+    }
+    self.pm.send('pandaStates', dat)
+
+  def update(self, simulator_state: SimulatorState):
+    self.send_can_messages(simulator_state)
+
+    if self.idx % 50 == 0: # only send panda states at 2hz
+      self.send_panda_state(simulator_state)
+
+    self.idx += 1

tools/sim/lib/simulated_sensors.py

@@ -0,0 +1,125 @@
+import time
+
+from cereal import log
+import cereal.messaging as messaging
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_DMON
+from openpilot.tools.sim.lib.camerad import Camerad
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+  from openpilot.tools.sim.lib.common import World, SimulatorState
+
+
+class SimulatedSensors:
+  """Simulates the C3 sensors (acc, gyro, gps, peripherals, dm state, cameras) to OpenPilot"""
+
+  def __init__(self, dual_camera=False):
+    self.pm = messaging.PubMaster(['accelerometer', 'gyroscope', 'gpsLocationExternal', 'driverStateV2', 'driverMonitoringState', 'peripheralState'])
+    self.camerad = Camerad(dual_camera=dual_camera)
+    self.last_perp_update = 0
+    self.last_dmon_update = 0
+
+  def send_imu_message(self, simulator_state: 'SimulatorState'):
+    for _ in range(5):
+      dat = messaging.new_message('accelerometer')
+      dat.accelerometer.sensor = 4
+      dat.accelerometer.type = 0x10
+      dat.accelerometer.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
+      dat.accelerometer.init('acceleration')
+      dat.accelerometer.acceleration.v = [simulator_state.imu.accelerometer.x, simulator_state.imu.accelerometer.y, simulator_state.imu.accelerometer.z]
+      self.pm.send('accelerometer', dat)
+
+      # copied these numbers from locationd
+      dat = messaging.new_message('gyroscope')
+      dat.gyroscope.sensor = 5
+      dat.gyroscope.type = 0x10
+      dat.gyroscope.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
+      dat.gyroscope.init('gyroUncalibrated')
+      dat.gyroscope.gyroUncalibrated.v = [simulator_state.imu.gyroscope.x, simulator_state.imu.gyroscope.y, simulator_state.imu.gyroscope.z]
+      self.pm.send('gyroscope', dat)
+
+  def send_gps_message(self, simulator_state: 'SimulatorState'):
+    if not simulator_state.valid:
+      return
+
+    # transform vel from carla to NED
+    # north is -Y in CARLA
+    velNED = [
+      -simulator_state.velocity.y,  # north/south component of NED is negative when moving south
+      simulator_state.velocity.x,  # positive when moving east, which is x in carla
+      simulator_state.velocity.z,
+    ]
+
+    for _ in range(10):
+      dat = messaging.new_message('gpsLocationExternal')
+      dat.gpsLocationExternal = {
+        "unixTimestampMillis": int(time.time() * 1000),
+        "flags": 1,  # valid fix
+        "accuracy": 1.0,
+        "verticalAccuracy": 1.0,
+        "speedAccuracy": 0.1,
+        "bearingAccuracyDeg": 0.1,
+        "vNED": velNED,
+        "bearingDeg": simulator_state.imu.bearing,
+        "latitude": simulator_state.gps.latitude,
+        "longitude": simulator_state.gps.longitude,
+        "altitude": simulator_state.gps.altitude,
+        "speed": simulator_state.speed,
+        "source": log.GpsLocationData.SensorSource.ublox,
+      }
+
+      self.pm.send('gpsLocationExternal', dat)
+
+  def send_peripheral_state(self):
+    dat = messaging.new_message('peripheralState')
+    dat.valid = True
+    dat.peripheralState = {
+      'pandaType': log.PandaState.PandaType.blackPanda,
+      'voltage': 12000,
+      'current': 5678,
+      'fanSpeedRpm': 1000
+    }
+    Params().put_bool("ObdMultiplexingEnabled", False)
+    self.pm.send('peripheralState', dat)
+
+  def send_fake_driver_monitoring(self):
+    # dmonitoringmodeld output
+    dat = messaging.new_message('driverStateV2')
+    dat.driverStateV2.leftDriverData.faceOrientation = [0., 0., 0.]
+    dat.driverStateV2.leftDriverData.faceProb = 1.0
+    dat.driverStateV2.rightDriverData.faceOrientation = [0., 0., 0.]
+    dat.driverStateV2.rightDriverData.faceProb = 1.0
+    self.pm.send('driverStateV2', dat)
+
+    # dmonitoringd output
+    dat = messaging.new_message('driverMonitoringState')
+    dat.driverMonitoringState = {
+      "faceDetected": True,
+      "isDistracted": False,
+      "awarenessStatus": 1.,
+    }
+    self.pm.send('driverMonitoringState', dat)
+
+  def send_camera_images(self, world: 'World'):
+    with world.image_lock:
+      yuv = self.camerad.rgb_to_yuv(world.road_image)
+      self.camerad.cam_send_yuv_road(yuv)
+
+      if world.dual_camera:
+        yuv = self.camerad.rgb_to_yuv(world.wide_road_image)
+        self.camerad.cam_send_yuv_wide_road(yuv)
+
+  def update(self, simulator_state: 'SimulatorState', world: 'World'):
+    now = time.time()
+    self.send_imu_message(simulator_state)
+    self.send_gps_message(simulator_state)
+
+    if (now - self.last_dmon_update) > DT_DMON/2:
+      self.send_fake_driver_monitoring()
+      self.last_dmon_update = now
+
+    if (now - self.last_perp_update) > 0.25:
+      self.send_peripheral_state()
+      self.last_perp_update = now