HKG: Add FW for 2020 Genesis G70 (#24045)

* Remove max steer

* Update ref

.pylintrc

@@ -54,7 +54,7 @@ confidence=
 # --enable=similarities". If you want to run only the classes checker, but have
 # no Warning level messages displayed, use"--disable=all --enable=classes
 # --disable=W"
-disable=C,R,W0613,W0511,W0212,W0201,W0311,W0106,W0603,W0621,W0703,W1201,W1203,E1136,W1514
+disable=C,R,W0613,W0511,W0212,W0201,W0106,W0603,W0621,W0703,W1201,W1203,E1136,W1514
 
 
 # Enable the message, report, category or checker with the given id(s). You can

cereal

@@ -1 +1 @@
-Subproject commit 447d8b8a7a9c75e5f3478ac08622d6f1d2121528
+Subproject commit a818779f1a55c6bf53fa0692e31111585662b722

common/kalman/tests/test_simple_kalman.py

@@ -36,8 +36,8 @@ class TestSimpleKalman(unittest.TestCase):
     self.assertEqual(self.kf.x, [[1.0], [1.0]])
 
   def update_returns_state(self):
-      x = self.kf.update(100)
-      self.assertEqual(x, self.kf.x)
+    x = self.kf.update(100)
+    self.assertEqual(x, self.kf.x)
 
   def test_old_equal_new(self):
     for _ in range(1000):

common/logging_extra.py

@@ -172,33 +172,33 @@ class SwagLogger(logging.Logger):
     #On some versions of IronPython, currentframe() returns None if
     #IronPython isn't run with -X:Frames.
     if f is not None:
-        f = f.f_back
+      f = f.f_back
     orig_f = f
     while f and stacklevel > 1:
-        f = f.f_back
-        stacklevel -= 1
+      f = f.f_back
+      stacklevel -= 1
     if not f:
-        f = orig_f
+      f = orig_f
     rv = "(unknown file)", 0, "(unknown function)", None
     while hasattr(f, "f_code"):
-        co = f.f_code
-        filename = os.path.normcase(co.co_filename)
+      co = f.f_code
+      filename = os.path.normcase(co.co_filename)
 
-        # TODO: is this pylint exception correct?
-        if filename == _srcfile:  # pylint: disable=comparison-with-callable
-            f = f.f_back
-            continue
-        sinfo = None
-        if stack_info:
-            sio = io.StringIO()
-            sio.write('Stack (most recent call last):\n')
-            traceback.print_stack(f, file=sio)
-            sinfo = sio.getvalue()
-            if sinfo[-1] == '\n':
-                sinfo = sinfo[:-1]
-            sio.close()
-        rv = (co.co_filename, f.f_lineno, co.co_name, sinfo)
-        break
+      # TODO: is this pylint exception correct?
+      if filename == _srcfile:  # pylint: disable=comparison-with-callable
+        f = f.f_back
+        continue
+      sinfo = None
+      if stack_info:
+        sio = io.StringIO()
+        sio.write('Stack (most recent call last):\n')
+        traceback.print_stack(f, file=sio)
+        sinfo = sio.getvalue()
+        if sinfo[-1] == '\n':
+          sinfo = sinfo[:-1]
+        sio.close()
+      rv = (co.co_filename, f.f_lineno, co.co_name, sinfo)
+      break
     return rv
 
 if __name__ == "__main__":

models/README.md

@@ -9,6 +9,12 @@ To view the architecture of the ONNX networks, you can use [netron](https://netr
       * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
       * Channel 4 represents the half-res U channel
       * Channel 5 represents the half-res V channel
+* **wide image stream**
+  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
+    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
+      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
+      * Channel 4 represents the half-res U channel
+      * Channel 5 represents the half-res V channel
 * **desire**
   * one-hot encoded vector to command model to execute certain actions, bit only needs to be sent for 1 frame : 8
 * **traffic convention**

panda

@@ -1 +1 @@
-Subproject commit 499906f3244712c1edd7f1cc8e92112d11f2b505
+Subproject commit 7104818e148494523e87fd74736748b6ffd0a7c4

selfdrive/assets/compress-images.sh

@@ -2,3 +2,6 @@
 
 echo "compressing training guide images"
 optipng -o7 -strip all training/* training_wide/*
+
+# This can sometimes provide smaller images
+# mogrify -quality 100 -format jpg training_wide/* training/*

selfdrive/boardd/boardd.cc

@@ -161,7 +161,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
   int safety_param;
 
   auto safety_configs = car_params.getSafetyConfigs();
-  uint16_t unsafe_mode = car_params.getUnsafeMode();
+  uint16_t alternative_experience = car_params.getAlternativeExperience();
   for (uint32_t i = 0; i < pandas.size(); i++) {
     auto panda = pandas[i];
 
@@ -174,8 +174,8 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
       safety_param = 0;
     }
 
-    LOGW("panda %d: setting safety model: %d, param: %d, unsafe mode: %d", i, (int)safety_model, safety_param, unsafe_mode);
-    panda->set_unsafe_mode(unsafe_mode);
+    LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
+    panda->set_alternative_experience(alternative_experience);
     panda->set_safety_model(safety_model, safety_param);
   }
 
@@ -362,7 +362,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt));
     ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
     ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
-    ps.setUnsafeMode(health.unsafe_mode_pkt);
+    ps.setAlternativeExperience(health.alternative_experience_pkt);
     ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
 
     // Convert faults bitset to capnp list

selfdrive/boardd/panda.cc

@@ -251,8 +251,8 @@ void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int sa
   usb_write(0xdc, (uint16_t)safety_model, safety_param);
 }
 
-void Panda::set_unsafe_mode(uint16_t unsafe_mode) {
-  usb_write(0xdf, unsafe_mode, 0);
+void Panda::set_alternative_experience(uint16_t alternative_experience) {
+  usb_write(0xdf, alternative_experience, 0);
 }
 
 cereal::PandaState::PandaType Panda::get_hw_type() {

selfdrive/boardd/panda.h

@@ -74,7 +74,7 @@ class Panda {
   // Panda functionality
   cereal::PandaState::PandaType get_hw_type();
   void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
-  void set_unsafe_mode(uint16_t unsafe_mode);
+  void set_alternative_experience(uint16_t alternative_experience);
   void set_rtc(struct tm sys_time);
   struct tm get_rtc();
   void set_fan_speed(uint16_t fan_speed);

selfdrive/boardd/tests/test_boardd_api.py

@@ -74,4 +74,4 @@ class TestBoarddApiMethods(unittest.TestCase):
 
 
 if __name__ == '__main__':
-    unittest.main()
+  unittest.main()

selfdrive/camerad/test/get_thumbnails_for_segment.py

@@ -24,9 +24,9 @@ if __name__ == "__main__":
   lr = list(LogReader(path))
 
   for msg in tqdm(lr):
-      if msg.which() == 'thumbnail':
-          with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
-              f.write(msg.thumbnail.thumbnail)
-      elif msg.which() == 'navThumbnail':
-          with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
-              f.write(msg.navThumbnail.thumbnail)
+    if msg.which() == 'thumbnail':
+      with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
+        f.write(msg.thumbnail.thumbnail)
+    elif msg.which() == 'navThumbnail':
+      with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
+        f.write(msg.navThumbnail.thumbnail)

selfdrive/car/honda/interface.py

@@ -404,7 +404,7 @@ class CarInterface(CarInterfaceBase):
       # do enable on both accel and decel buttons
       if not self.CP.pcmCruise:
         if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed:
-            events.add(EventName.buttonEnable)
+          events.add(EventName.buttonEnable)
 
       # do disable on button down
       if b.type == ButtonType.cancel and b.pressed:

selfdrive/car/hyundai/values.py

@@ -753,10 +753,12 @@ FW_VERSIONS = {
   CAR.GENESIS_G70_2020: {
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00IK  MDPS R 1.00 1.07 57700-G9220 4I2VL107',
+      b'\xf1\x00IK  MDPS R 1.00 1.07 57700-G9420 4I4VL107',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00',
       b'\x00\x00\x00\x00\xf1\x00bcsh8p54  E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL',
+      b'\xf1\x87VDKLT18912362DN4wfVfwefeveVUwfvw\x88vWfvUFU\x89\xa9\x8f\xff\x87w\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00IK__ SCC F-CUP      1.00 1.02 96400-G9100         ',

selfdrive/car/interfaces.py

@@ -75,8 +75,6 @@ class CarInterfaceBase(ABC):
 
     # standard ALC params
     ret.steerControlType = car.CarParams.SteerControlType.torque
-    ret.steerMaxBP = [0.]
-    ret.steerMaxV = [1.]
     ret.minSteerSpeed = 0.
     ret.wheelSpeedFactor = 1.0
 

selfdrive/car/nissan/carcontroller.py

@@ -58,14 +58,14 @@ class CarController():
     self.last_angle = apply_angle
 
     if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel:
-        can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame))
+      can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame))
 
     # TODO: Find better way to cancel!
     # For some reason spamming the cancel button is unreliable on the Leaf
     # We now cancel by making propilot think the seatbelt is unlatched,
     # this generates a beep and a warning message every time you disengage
     if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and frame % 2 == 0:
-        can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
+      can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
 
     can_sends.append(nissancan.create_steering_control(
         self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque))

selfdrive/car/subaru/subarucan.py

@@ -54,7 +54,7 @@ def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_
     values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
     values["LKAS_Dash_State"] = 2 # Green enabled indicator
   else:
-     values["LKAS_Dash_State"] = 0 # LKAS Not enabled
+    values["LKAS_Dash_State"] = 0 # LKAS Not enabled
 
   values["LKAS_Left_Line_Visible"] = int(left_line)
   values["LKAS_Right_Line_Visible"] = int(right_line)

selfdrive/car/tests/test_fingerprints.py

@@ -37,12 +37,12 @@ def check_fingerprint_consistency(f1, f2):
   is_f1_in_f2 = True
   for k in f1:
     if (k not in f2 or f1[k] != f2[k]) and k < max_msg:
-       is_f1_in_f2 = False
+      is_f1_in_f2 = False
 
   is_f2_in_f1 = True
   for k in f2:
     if (k not in f1 or f2[k] != f1[k]) and k < max_msg:
-       is_f2_in_f1 = False
+      is_f2_in_f1 = False
 
   return not is_f1_in_f2 and not is_f2_in_f1
 

selfdrive/controls/controlsd.py

@@ -94,7 +94,7 @@ class Controls:
     get_one_can(self.can_sock)
 
     self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
-    self.CP.unsafeMode = 0  # see panda/board/safety_declarations.h for allowed values
+    self.CP.alternativeExperience = 0  # see panda/board/safety_declarations.h for allowed values
 
     # read params
     self.is_metric = params.get_bool("IsMetric")
@@ -260,7 +260,7 @@ class Controls:
       if i < len(self.CP.safetyConfigs):
         safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
                           pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
-                          pandaState.unsafeMode != self.CP.unsafeMode
+                          pandaState.alternativeExperience != self.CP.alternativeExperience
       else:
         safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
 

selfdrive/controls/lib/drive_helpers.py

@@ -40,10 +40,6 @@ def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
 
-def get_steer_max(CP, v_ego):
-  return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
-
-
 def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
   # handle button presses. TODO: this should be in state_control, but a decelCruise press
   # would have the effect of both enabling and changing speed is checked after the state transition

selfdrive/controls/lib/latcontrol.py

@@ -12,6 +12,9 @@ class LatControl(ABC):
     self.sat_limit = CP.steerLimitTimer
     self.sat_count = 0.
 
+    # we define the steer torque scale as [-1.0...1.0]
+    self.steer_max = 1.0
+
   @abstractmethod
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     pass

selfdrive/controls/lib/latcontrol_indi.py

@@ -5,7 +5,6 @@ from cereal import log
 from common.filter_simple import FirstOrderFilter
 from common.numpy_fast import clip, interp
 from common.realtime import DT_CTRL
-from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 
 
@@ -41,7 +40,6 @@ class LatControlINDI(LatControl):
     self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)
 
     self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
-
     self.reset()
 
   @property
@@ -107,8 +105,7 @@ class LatControlINDI(LatControl):
 
       output_steer = self.steer_filter.x + delta_u
 
-      steers_max = get_steer_max(CP, CS.vEgo)
-      output_steer = clip(output_steer, -steers_max, steers_max)
+      output_steer = clip(output_steer, -self.steer_max, self.steer_max)
 
       indi_log.active = True
       indi_log.rateSetPoint = float(rate_sp)
@@ -117,6 +114,6 @@ class LatControlINDI(LatControl):
       indi_log.delayedOutput = float(self.steer_filter.x)
       indi_log.delta = float(delta_u)
       indi_log.output = float(output_steer)
-      indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
+      indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
 
     return float(output_steer), float(steers_des), indi_log

selfdrive/controls/lib/latcontrol_lqr.py

@@ -4,7 +4,6 @@ import numpy as np
 from common.numpy_fast import clip
 from common.realtime import DT_CTRL
 from cereal import log
-from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 
 
@@ -34,7 +33,6 @@ class LatControlLQR(LatControl):
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
 
-    steers_max = get_steer_max(CP, CS.vEgo)
     torque_scale = (0.45 + CS.vEgo / 60.0)**2  # Scale actuator model with speed
 
     # Subtract offset. Zero angle should correspond to zero torque
@@ -71,16 +69,16 @@ class LatControlLQR(LatControl):
         i = self.i_lqr + self.ki * self.i_rate * error
         control = lqr_output + i
 
-        if (error >= 0 and (control <= steers_max or i < 0.0)) or \
-           (error <= 0 and (control >= -steers_max or i > 0.0)):
+        if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
+           (error <= 0 and (control >= -self.steer_max or i > 0.0)):
           self.i_lqr = i
 
       output_steer = lqr_output + self.i_lqr
-      output_steer = clip(output_steer, -steers_max, steers_max)
+      output_steer = clip(output_steer, -self.steer_max, self.steer_max)
 
     lqr_log.steeringAngleDeg = angle_steers_k
     lqr_log.i = self.i_lqr
     lqr_log.output = output_steer
     lqr_log.lqrOutput = lqr_output
-    lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
+    lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
     return output_steer, desired_angle, lqr_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,7 +1,6 @@
 import math
 
 from selfdrive.controls.lib.pid import PIController
-from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 from cereal import log
 
@@ -33,9 +32,8 @@ class LatControlPID(LatControl):
       pid_log.active = False
       self.pid.reset()
     else:
-      steers_max = get_steer_max(CP, CS.vEgo)
-      self.pid.pos_limit = steers_max
-      self.pid.neg_limit = -steers_max
+      self.pid.pos_limit = self.steer_max
+      self.pid.neg_limit = -self.steer_max
 
       # offset does not contribute to resistive torque
       steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
@@ -49,6 +47,6 @@ class LatControlPID(LatControl):
       pid_log.i = self.pid.i
       pid_log.f = self.pid.f
       pid_log.output = output_steer
-      pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
+      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
 
     return output_steer, angle_steers_des, pid_log

selfdrive/debug/show_matching_cars.py

@@ -10,11 +10,11 @@ candidate_cars = all_legacy_fingerprint_cars()
 
 
 for addr, l in fingerprint.items():
-    dat = messaging.new_message('can', 1)
+  dat = messaging.new_message('can', 1)
 
-    msg = dat.can[0]
-    msg.address = addr
-    msg.dat = " " * l
+  msg = dat.can[0]
+  msg.address = addr
+  msg.dat = " " * l
 
-    candidate_cars = eliminate_incompatible_cars(msg, candidate_cars)
-    print(candidate_cars)
+  candidate_cars = eliminate_incompatible_cars(msg, candidate_cars)
+  print(candidate_cars)

selfdrive/hardware/tici/hardware.py

@@ -24,19 +24,19 @@ MM_MODEM_SIMPLE = MM + ".Modem.Simple"
 MM_SIM = MM + ".Sim"
 
 class MM_MODEM_STATE(IntEnum):
-       FAILED        = -1
-       UNKNOWN       = 0
-       INITIALIZING  = 1
-       LOCKED        = 2
-       DISABLED      = 3
-       DISABLING     = 4
-       ENABLING      = 5
-       ENABLED       = 6
-       SEARCHING     = 7
-       REGISTERED    = 8
-       DISCONNECTING = 9
-       CONNECTING    = 10
-       CONNECTED     = 11
+  FAILED        = -1
+  UNKNOWN       = 0
+  INITIALIZING  = 1
+  LOCKED        = 2
+  DISABLED      = 3
+  DISABLING     = 4
+  ENABLING      = 5
+  ENABLED       = 6
+  SEARCHING     = 7
+  REGISTERED    = 8
+  DISCONNECTING = 9
+  CONNECTING    = 10
+  CONNECTED     = 11
 
 class NMMetered(IntEnum):
   NM_METERED_UNKNOWN = 0
@@ -54,6 +54,11 @@ NetworkStrength = log.DeviceState.NetworkStrength
 MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5
 MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14
 
+
+def sudo_write(val, path):
+  os.system(f"sudo su -c 'echo {val} > {path}'")
+
+
 class Tici(HardwareBase):
   @cached_property
   def bus(self):
@@ -194,14 +199,14 @@ class Tici(HardwareBase):
       return None
 
   def parse_strength(self, percentage):
-      if percentage < 25:
-        return NetworkStrength.poor
-      elif percentage < 50:
-        return NetworkStrength.moderate
-      elif percentage < 75:
-        return NetworkStrength.good
-      else:
-        return NetworkStrength.great
+    if percentage < 25:
+      return NetworkStrength.poor
+    elif percentage < 50:
+      return NetworkStrength.moderate
+    elif percentage < 75:
+      return NetworkStrength.good
+    else:
+      return NetworkStrength.great
 
   def get_network_strength(self, network_type):
     network_strength = NetworkStrength.unknown
@@ -390,18 +395,16 @@ class Tici(HardwareBase):
     if not powersave_enabled:
       self.amplifier.initialize_configuration()
 
+    # *** CPU config ***
+
     # offline big cluster, leave core 4 online for boardd
     for i in range(5, 8):
-      # TODO: fix permissions with udev
       val = "0" if powersave_enabled else "1"
-      os.system(f"sudo su -c 'echo {val} > /sys/devices/system/cpu/cpu{i}/online'")
+      sudo_write(val, f"/sys/devices/system/cpu/cpu{i}/online")
 
     for n in ('0', '4'):
-      gov = 'userspace' if powersave_enabled else 'performance'
-      os.system(f"sudo su -c 'echo {gov} > /sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor'")
-
-      if powersave_enabled:
-        os.system(f"sudo su -c 'echo 979200 > /sys/devices/system/cpu/cpufreq/policy{n}/scaling_setspeed'")
+      gov = 'ondemand' if powersave_enabled else 'performance'
+      sudo_write(gov, f"/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor")
 
   def get_gpu_usage_percent(self):
     try:
@@ -416,9 +419,19 @@ class Tici(HardwareBase):
     # Allow thermald to write engagement status to kmsg
     os.system("sudo chmod a+w /dev/kmsg")
 
+    # *** GPU config ***
+    sudo_write("0", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel")
+    sudo_write("0", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on")
+    sudo_write("1000000", "/sys/class/kgsl/kgsl-3d0/idle_timer")
+    sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor")
+
     # setup governors
-    os.system("sudo su -c 'echo performance > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor'")
-    os.system("sudo su -c 'echo performance > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor'")
+    sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor")
+    sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu0/governor")
+    sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu4/governor")
 
   def get_networks(self):
     r = {}

selfdrive/locationd/models/loc_kf.py

@@ -474,14 +474,14 @@ class LocKalman():
     z = trans[:, :3]
     R = np.zeros((len(trans), 3, 3))
     for i, _ in enumerate(z):
-        R[i, :, :] = np.diag(trans[i, 3:]**2)
+      R[i, :, :] = np.diag(trans[i, 3:]**2)
     return self.filter.predict_and_update_batch(t, kind, z, R)
 
   def predict_and_update_odo_rot(self, rot, t, kind):
     z = rot[:, :3]
     R = np.zeros((len(rot), 3, 3))
     for i, _ in enumerate(z):
-        R[i, :, :] = np.diag(rot[i, 3:]**2)
+      R[i, :, :] = np.diag(rot[i, 3:]**2)
     return self.filter.predict_and_update_batch(t, kind, z, R)
 
   def predict_and_update_orb_features(self, tracks, t, kind):
@@ -518,7 +518,7 @@ class LocKalman():
 
     y_full = np.zeros((z.shape[0], z.shape[1] - 3))
     if sum(good_idxs) > 0:
-        y_full[good_idxs] = np.array(ret[6])
+      y_full[good_idxs] = np.array(ret[6])
     ret = ret[:6] + (y_full, z, ecef_pos)
     return ret
 

selfdrive/loggerd/tests/fill_eon.py

@@ -23,4 +23,4 @@ if __name__ == "__main__":
     # Fill up to 99 percent
     available_percent = get_available_percent()
     if available_percent < 1.0:
-        break
+      break

selfdrive/loggerd/tests/loggerd_tests_common.py

@@ -8,22 +8,22 @@ import unittest
 import selfdrive.loggerd.uploader as uploader
 
 def create_random_file(file_path, size_mb, lock=False):
-    try:
-      os.mkdir(os.path.dirname(file_path))
-    except OSError:
-      pass
+  try:
+    os.mkdir(os.path.dirname(file_path))
+  except OSError:
+    pass
 
-    lock_path = file_path + ".lock"
-    if lock:
-      os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL))
+  lock_path = file_path + ".lock"
+  if lock:
+    os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL))
 
-    chunks = 128
-    chunk_bytes = int(size_mb * 1024 * 1024 / chunks)
-    data = os.urandom(chunk_bytes)
+  chunks = 128
+  chunk_bytes = int(size_mb * 1024 * 1024 / chunks)
+  data = os.urandom(chunk_bytes)
 
-    with open(file_path, 'wb') as f:
-      for _ in range(chunks):
-        f.write(data)
+  with open(file_path, 'wb') as f:
+    for _ in range(chunks):
+      f.write(data)
 
 class MockResponse():
   def __init__(self, text, status_code):

selfdrive/manager/process.py

@@ -199,7 +199,7 @@ class NativeProcess(ManagerProcess):
   def start(self) -> None:
     # In case we only tried a non blocking stop we need to stop it before restarting
     if self.shutting_down:
-        self.stop()
+      self.stop()
 
     if self.proc is not None:
       return
@@ -231,7 +231,7 @@ class PythonProcess(ManagerProcess):
   def start(self) -> None:
     # In case we only tried a non blocking stop we need to stop it before restarting
     if self.shutting_down:
-        self.stop()
+      self.stop()
 
     if self.proc is not None:
       return

selfdrive/manager/process_config.py

@@ -42,6 +42,9 @@ procs = [
   PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
   PythonProcess("shutdownd", "selfdrive.hardware.eon.shutdownd", enabled=EON),
   PythonProcess("androidd", "selfdrive.hardware.eon.androidd", enabled=EON, persistent=True),
+
+  # Experimental
+  PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=os.path.isfile("/persist/comma/use-quectel-rawgps")),
 ]
 
 managed_processes = {p.name: p for p in procs}

selfdrive/monitoring/test_monitoring.py

@@ -136,21 +136,21 @@ class TestMonitoring(unittest.TestCase):
   # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
   #  - both actions should clear the alert, but momentary appearance should not
   def test_sometimes_transparent_commuter(self):
-      _visible_time = np.random.choice([0.5, 10])
-      ds_vector = always_no_face[:]*2
-      interaction_vector = always_false[:]*2
-      ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
-      interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
-      events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
-      self.assertTrue(len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0)
-      self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
-      self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0)
-      if _visible_time == 0.5:
-        self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
-        self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0], EventName.preDriverUnresponsive)
-      elif _visible_time == 10:
-        self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
-        self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0)
+    _visible_time = np.random.choice([0.5, 10])
+    ds_vector = always_no_face[:]*2
+    interaction_vector = always_false[:]*2
+    ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
+    interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
+    events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
+    self.assertTrue(len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0)
+    self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
+    self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0)
+    if _visible_time == 0.5:
+      self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
+      self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0], EventName.preDriverUnresponsive)
+    elif _visible_time == 10:
+      self.assertEqual(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
+      self.assertTrue(len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0)
 
   # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
   #  - only disengage will clear the alert

selfdrive/sensord/rawgps/compare.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+import cereal.messaging as messaging
+from laika import constants
+
+if __name__ == "__main__":
+  sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
+
+  meas = None
+  while 1:
+    sm.update()
+    if sm['ubloxGnss'].which() == "measurementReport":
+      meas = sm['ubloxGnss'].measurementReport.measurements
+    if not sm.updated['qcomGnss'] or meas is None:
+      continue
+    report = sm['qcomGnss'].measurementReport
+    if report.source not in [0, 1]:
+      continue
+    GLONASS = report.source == 1
+    recv_time = report.milliseconds / 1000
+
+    car = []
+    print("qcom has ", list(sorted([x.svId for x in report.sv])))
+    print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)])))
+    for i in report.sv:
+      # match to ublox
+      tm = None
+      for m in meas:
+        if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
+          tm = m
+        if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
+          tm = m
+      if tm is None:
+        continue
+      
+      if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
+        sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
+        ublox_psuedorange = tm.pseudorange
+        qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
+        if GLONASS:
+          glonass_freq = tm.glonassFrequencyIndex - 7
+          ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
+        else:
+          ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
+        qcom_speed = i.unfilteredSpeed
+        car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
+
+    if len(car) == 0:
+      print("nothing to compare")
+      continue
+
+    pr_err, speed_err = 0., 0.
+    for c in car:
+      ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
+      pr_err += ublox_psuedorange - qcom_psuedorange
+      speed_err += ublox_speed - qcom_speed
+    pr_err /= len(car)
+    speed_err /= len(car)
+    print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
+    for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
+      svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
+      print("svid: %3d  pseudorange: %10.2f m  speed: %8.2f m/s   meas: %12.2f  speed: %10.2f   meas_err: %10.3f speed_err: %8.3f cno: %d" %
+        (svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
+        ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
+
+
+

selfdrive/sensord/rawgps/modemdiag.py

@@ -0,0 +1,101 @@
+import os
+import time
+from serial import Serial
+from crcmod import mkCrcFun
+from struct import pack, unpack_from, calcsize
+
+class ModemDiag:
+  def __init__(self):
+    self.serial = self.open_serial()
+
+  def open_serial(self):
+    def op():
+      return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True)
+    try:
+      serial = op()
+    except Exception:
+      # TODO: this is a hack to get around modemmanager's exclusive open
+      print("unlocking serial...")
+      os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'')
+      os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'')
+      time.sleep(0.5)
+      os.system("sudo chmod 666 /dev/ttyUSB0")
+      serial = op()
+    serial.flush()
+    return serial
+
+  ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
+  ESCAPE_CHAR = b'\x7d'
+  TRAILER_CHAR = b'\x7e'
+
+  def hdlc_encapsulate(self, payload):
+    payload += pack('<H', ModemDiag.ccitt_crc16(payload))
+    payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]))
+    payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]))
+    payload += self.TRAILER_CHAR
+    return payload
+
+  def hdlc_decapsulate(self, payload):
+    assert len(payload) >= 3
+    assert payload[-1:] == self.TRAILER_CHAR
+    payload = payload[:-1]
+    payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR)
+    payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR)
+    assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2]))
+    return payload[:-2]
+
+  def recv(self):
+    raw_payload = []
+    while 1:
+      char_read = self.serial.read()
+      raw_payload.append(char_read)
+      if char_read.endswith(self.TRAILER_CHAR):
+        break
+    raw_payload = b''.join(raw_payload)
+    unframed_message = self.hdlc_decapsulate(raw_payload)
+    return unframed_message[0], unframed_message[1:]
+
+  def send(self, packet_type, packet_payload):
+    self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload))
+
+# *** end class ***
+
+DIAG_LOG_F = 16
+DIAG_LOG_CONFIG_F = 115
+LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1
+LOG_CONFIG_SET_MASK_OP = 3
+LOG_CONFIG_SUCCESS_S = 0
+
+def send_recv(diag, packet_type, packet_payload):
+  diag.send(packet_type, packet_payload)
+  while 1:
+    opcode, payload = diag.recv()
+    if opcode != DIAG_LOG_F:
+      break
+  return opcode, payload
+
+def setup_logs(diag, types_to_log):
+  opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP))
+
+  header_spec = '<3xII'
+  operation, status = unpack_from(header_spec, payload)
+  assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP
+  assert status == LOG_CONFIG_SUCCESS_S
+
+  log_masks = unpack_from('<16I', payload, calcsize(header_spec))
+
+  for log_type, log_mask_bitsize in enumerate(log_masks):
+    if log_mask_bitsize:
+      log_mask = [0] * ((log_mask_bitsize+7)//8)
+      for i in range(log_mask_bitsize):
+        if ((log_type<<12)|i) in types_to_log:
+          log_mask[i//8] |= 1 << (i%8)
+      opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII',
+          LOG_CONFIG_SET_MASK_OP,
+          log_type,
+          log_mask_bitsize
+      ) + bytes(log_mask))
+      assert opcode == DIAG_LOG_CONFIG_F
+      operation, status = unpack_from(header_spec, payload)
+      assert operation == LOG_CONFIG_SET_MASK_OP
+      assert status == LOG_CONFIG_SUCCESS_S

selfdrive/sensord/rawgps/rawgpsd.py

@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+import os
+import sys
+import signal
+import itertools
+import math
+from typing import NoReturn
+from struct import unpack_from, calcsize
+
+import cereal.messaging as messaging
+from cereal import log
+from selfdrive.swaglog import cloudlog
+
+from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs
+from selfdrive.sensord.rawgps.structs import dict_unpacker
+from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv
+from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv
+from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT
+from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT
+
+miscStatusFields = {
+  "multipathEstimateIsValid": 0,
+  "directionIsValid": 1,
+}
+
+measurementStatusFields = {
+  "subMillisecondIsValid": 0,
+  "subBitTimeIsKnown": 1,
+  "satelliteTimeIsKnown": 2,
+  "bitEdgeConfirmedFromSignal": 3,
+  "measuredVelocity": 4,
+  "fineOrCoarseVelocity": 5,
+  "lockPointValid": 6,
+  "lockPointPositive": 7,
+
+  "lastUpdateFromDifference": 9,
+  "lastUpdateFromVelocityDifference": 10,
+  "strongIndicationOfCrossCorelation": 11,
+  "tentativeMeasurement": 12,
+  "measurementNotUsable": 13,
+  "sirCheckIsNeeded": 14,
+  "probationMode": 15,
+
+  "multipathIndicator": 24,
+  "imdJammingIndicator": 25,
+  "lteB13TxJammingIndicator": 26,
+  "freshMeasurementIndicator": 27,
+}
+
+measurementStatusGPSFields = {
+  "gpsRoundRobinRxDiversity": 18,
+  "gpsRxDiversity": 19,
+  "gpsLowBandwidthRxDiversityCombined": 20,
+  "gpsHighBandwidthNu4": 21,
+  "gpsHighBandwidthNu8": 22,
+  "gpsHighBandwidthUniform": 23,
+}
+
+measurementStatusGlonassFields = {
+  "glonassMeanderBitEdgeValid": 16,
+  "glonassTimeMarkValid": 17
+}
+
+def main() -> NoReturn:
+  unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
+  unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
+
+  unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
+  unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
+
+  log_types = [
+    LOG_GNSS_GPS_MEASUREMENT_REPORT,
+    LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
+  ]
+  pub_types = ['qcomGnss']
+  if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1:
+    unpack_position, _ = dict_unpacker(position_report)
+    log_types.append(LOG_GNSS_POSITION_REPORT)
+    pub_types.append("gpsLocationExternal")
+
+  os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
+  diag = ModemDiag()
+
+  def try_setup_logs(diag, log_types):
+    for _ in range(5):
+      try:
+        setup_logs(diag, log_types)
+        break
+      except Exception:
+        pass
+
+  def disable_logs(sig, frame):
+    os.system("mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea")
+    cloudlog.warning("rawgpsd: shutting down")
+    try_setup_logs(diag, [])
+    cloudlog.warning("rawgpsd: logs disabled")
+    sys.exit(0)
+  signal.signal(signal.SIGINT, disable_logs)
+  try_setup_logs(diag, log_types)
+  cloudlog.warning("rawgpsd: setup logs done")
+
+  pm = messaging.PubMaster(pub_types)
+
+  while 1:
+    opcode, payload = diag.recv()
+    assert opcode == DIAG_LOG_F
+    (pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
+    if pending_msgs > 0:
+      cloudlog.debug("have %d pending messages" % pending_msgs)
+    assert log_outer_length == len(inner_log_packet)
+    (log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
+    assert log_inner_length == len(inner_log_packet)
+    if log_type not in log_types:
+      continue
+    #print("got log: %x" % log_type)
+
+    if log_type == LOG_GNSS_POSITION_REPORT:
+      report = unpack_position(log_payload)
+      if report["u_PosSource"] != 2:
+        continue
+      vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
+      vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
+
+      msg = messaging.new_message('gpsLocationExternal')
+      gps = msg.gpsLocationExternal
+      gps.flags = 1
+      gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
+      gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
+      gps.altitude = report["q_FltFinalPosAlt"]
+      gps.speed = math.sqrt(sum([x**2 for x in vNED]))
+      gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
+      # TODO: this probably isn't right, use laika for this
+      gps.timestamp = report['w_GpsWeekNumber']*604800*1000 + report['q_GpsFixTimeMs']
+      gps.source = log.GpsLocationData.SensorSource.qcomdiag
+      gps.vNED = vNED
+      gps.verticalAccuracy = report["q_FltVdop"]
+      gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi
+      gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
+
+      pm.send('gpsLocationExternal', msg)
+
+    if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
+      msg = messaging.new_message('qcomGnss')
+
+      gnss = msg.qcomGnss
+      gnss.logTs = log_time
+      gnss.init('measurementReport')
+      report = gnss.measurementReport
+
+      if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
+        dat = unpack_gps_meas(log_payload)
+        sats = log_payload[size_gps_meas:]
+        unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
+        report.source = 0  # gps
+        measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items())
+      elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
+        dat = unpack_glonass_meas(log_payload)
+        sats = log_payload[size_glonass_meas:]
+        unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
+        report.source = 1  # glonass
+        measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
+      else:
+        assert False
+
+      for k,v in dat.items():
+        if k == "version":
+          assert v == 0
+        elif k == "week":
+          report.gpsWeek = v
+        elif k == "svCount":
+          pass
+        else:
+          setattr(report, k, v)
+      assert len(sats)//dat['svCount'] == size_meas_sv
+      report.init('sv', dat['svCount'])
+      for i in range(dat['svCount']):
+        sv = report.sv[i]
+        sv.init('measurementStatus')
+        sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)])
+        for k,v in sat.items():
+          if k == "parityErrorCount":
+            sv.gpsParityErrorCount = v
+          elif k == "frequencyIndex":
+            sv.glonassFrequencyIndex = v
+          elif k == "hemmingErrorCount":
+            sv.glonassHemmingErrorCount = v
+          elif k == "measurementStatus":
+            for kk,vv in itertools.chain(*measurement_status_fields):
+              setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
+          elif k == "miscStatus":
+            for kk,vv in miscStatusFields.items():
+              setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
+          elif k == "pad":
+            pass
+          else:
+            setattr(sv, k, v)
+
+      pm.send('qcomGnss', msg)
+
+if __name__ == "__main__":
+  main()

selfdrive/sensord/rawgps/structs.py

@@ -0,0 +1,224 @@
+from struct import unpack_from, calcsize
+
+LOG_GNSS_POSITION_REPORT = 0x1476
+LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477
+LOG_GNSS_CLOCK_REPORT = 0x1478
+LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480
+LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756
+LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886
+
+LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE
+LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1
+
+LOG_GNSS_ME_DPO_STATUS = 0x1838
+LOG_GNSS_CD_DB_REPORT = 0x147B
+LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
+LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
+LOG_GNSS_CONFIGURATION_STATE = 0x1516
+
+glonass_measurement_report = """
+  uint8_t version;
+  uint32_t f_count;
+  uint8_t glonass_cycle_number;
+  uint16_t glonass_number_of_days;
+  uint32_t milliseconds;
+  float time_bias;
+  float clock_time_uncertainty;
+  float clock_frequency_bias;
+  float clock_frequency_uncertainty;
+  uint8_t sv_count;
+"""
+
+glonass_measurement_report_sv = """
+  uint8_t sv_id;
+  int8_t frequency_index;
+  uint8_t observation_state; // SVObservationStates
+  uint8_t observations;
+  uint8_t good_observations;
+  uint8_t hemming_error_count;
+  uint8_t filter_stages;
+  uint16_t carrier_noise;
+  int16_t latency;
+  uint8_t predetect_interval;
+  uint16_t postdetections;
+  uint32_t unfiltered_measurement_integral;
+  float unfiltered_measurement_fraction;
+  float unfiltered_time_uncertainty;
+  float unfiltered_speed;
+  float unfiltered_speed_uncertainty;
+  uint32_t measurement_status;
+  uint8_t misc_status;
+  uint32_t multipath_estimate;
+  float azimuth;
+  float elevation;
+  int32_t carrier_phase_cycles_integral;
+  uint16_t carrier_phase_cycles_fraction;
+  float fine_speed;
+  float fine_speed_uncertainty;
+  uint8_t cycle_slip_count;
+  uint32_t pad;
+"""
+
+gps_measurement_report = """
+  uint8_t version;
+  uint32_t f_count;
+  uint16_t week;
+  uint32_t milliseconds;
+  float time_bias;
+  float clock_time_uncertainty;
+  float clock_frequency_bias;
+  float clock_frequency_uncertainty;
+  uint8_t sv_count;
+"""
+
+gps_measurement_report_sv = """
+  uint8_t sv_id;
+  uint8_t observation_state; // SVObservationStates
+  uint8_t observations;
+  uint8_t good_observations;
+  uint16_t parity_error_count;
+  uint8_t filter_stages;
+  uint16_t carrier_noise;
+  int16_t latency;
+  uint8_t predetect_interval;
+  uint16_t postdetections;
+  uint32_t unfiltered_measurement_integral;
+  float unfiltered_measurement_fraction;
+  float unfiltered_time_uncertainty;
+  float unfiltered_speed;
+  float unfiltered_speed_uncertainty;
+  uint32_t measurement_status;
+  uint8_t misc_status;
+  uint32_t multipath_estimate;
+  float azimuth;
+  float elevation;
+  int32_t carrier_phase_cycles_integral;
+  uint16_t carrier_phase_cycles_fraction;
+  float fine_speed;
+  float fine_speed_uncertainty;
+  uint8_t cycle_slip_count;
+  uint32_t pad;
+"""
+
+position_report = """
+  uint8       u_Version;                /* Version number of DM log */
+  uint32      q_Fcount;                 /* Local millisecond counter */
+  uint8       u_PosSource;              /* Source of position information */ /*  0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database    */
+  uint32      q_Reserved1;              /* Reserved memory field */
+  uint16      w_PosVelFlag;             /* Position velocity bit field: (see DM log 0x1476 documentation) */
+  uint32      q_PosVelFlag2;            /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */
+  uint8       u_FailureCode;            /* Failure code: (see DM log 0x1476 documentation) */
+  uint16      w_FixEvents;              /* Fix events bit field: (see DM log 0x1476 documentation) */
+  uint32 _fake_align_week_number;
+  uint16      w_GpsWeekNumber;          /* GPS week number of position */
+  uint32      q_GpsFixTimeMs;           /* GPS fix time of week of in milliseconds */
+  uint8       u_GloNumFourYear;         /* Number of Glonass four year cycles */
+  uint16      w_GloNumDaysInFourYear;   /* Glonass calendar day in four year cycle */
+  uint32      q_GloFixTimeMs;           /* Glonass fix time of day in milliseconds */
+  uint32      q_PosCount;               /* Integer count of the number of unique positions reported */
+  uint64      t_DblFinalPosLatLon[2];   /* Final latitude and longitude of position in radians */
+  uint32      q_FltFinalPosAlt;         /* Final height-above-ellipsoid altitude of position */
+  uint32      q_FltHeadingRad;          /* User heading in radians */
+  uint32      q_FltHeadingUncRad;       /* User heading uncertainty in radians */
+  uint32      q_FltVelEnuMps[3];        /* User velocity in east, north, up coordinate frame. In meters per second. */
+  uint32      q_FltVelSigmaMps[3];      /* Gaussian 1-sigma value for east, north, up components of user velocity */
+  uint32      q_FltClockBiasMeters;     /* Receiver clock bias in meters */
+  uint32      q_FltClockBiasSigmaMeters;  /* Gaussian 1-sigma value for receiver clock bias in meters */
+  uint32      q_FltGGTBMeters;          /* GPS to Glonass time bias in meters */
+  uint32      q_FltGGTBSigmaMeters;     /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
+  uint32      q_FltGBTBMeters;          /* GPS to BeiDou time bias in meters */
+  uint32      q_FltGBTBSigmaMeters;     /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
+  uint32      q_FltBGTBMeters;          /* BeiDou to Glonass time bias in meters */
+  uint32      q_FltBGTBSigmaMeters;     /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
+  uint32      q_FltFiltGGTBMeters;      /* Filtered GPS to Glonass time bias in meters */
+  uint32      q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
+  uint32      q_FltFiltGBTBMeters;      /* Filtered GPS to BeiDou time bias in meters */
+  uint32      q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
+  uint32      q_FltFiltBGTBMeters;      /* Filtered BeiDou to Glonass time bias in meters */
+  uint32      q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
+  uint32      q_FltSftOffsetSec;        /* SFT offset as computed by WLS in seconds */
+  uint32      q_FltSftOffsetSigmaSec;   /* Gaussian 1-sigma value for SFT offset in seconds */
+  uint32      q_FltClockDriftMps;       /* Clock drift (clock frequency bias) in meters per second */
+  uint32      q_FltClockDriftSigmaMps;  /* Gaussian 1-sigma value for clock drift in meters per second */
+  uint32      q_FltFilteredAlt;         /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */
+  uint32      q_FltFilteredAltSigma;    /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */
+  uint32      q_FltRawAlt;              /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
+  uint32      q_FltRawAltSigma;         /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
+  uint32   align_Flt[14];
+  uint32      q_FltPdop;                /* 3D position dilution of precision as computed from the unweighted 
+  uint32      q_FltHdop;                /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
+  uint32      q_FltVdop;                /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
+  uint8       u_EllipseConfidence;      /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
+  uint32      q_FltEllipseAngle;        /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */
+  uint32      q_FltEllipseSemimajorAxis;  /* Semimajor axis of final horizontal position uncertainty error ellipse.  In units of meters. */
+  uint32      q_FltEllipseSemiminorAxis;  /* Semiminor axis of final horizontal position uncertainty error ellipse.  In units of meters. */
+  uint32      q_FltPosSigmaVertical;    /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */
+  uint8       u_HorizontalReliability;  /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High    */
+  uint8       u_VerticalReliability;    /* Vertical position reliability */
+  uint16      w_Reserved2;              /* Reserved memory field */
+  uint32      q_FltGnssHeadingRad;      /* User heading in radians derived from GNSS only solution  */
+  uint32      q_FltGnssHeadingUncRad;   /* User heading uncertainty in radians derived from GNSS only solution  */
+  uint32      q_SensorDataUsageMask;    /* Denotes which additional sensor data were used to compute this position fix.  BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/
+  uint32      q_SensorAidMask;         /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */
+  uint8       u_NumGpsSvsUsed;          /* The number of GPS SVs used in the fix */
+  uint8       u_TotalGpsSvs;            /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */
+  uint8       u_NumGloSvsUsed;          /* The number of Glonass SVs used in the fix */
+  uint8       u_TotalGloSvs;            /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
+  uint8       u_NumBdsSvsUsed;          /* The number of BeiDou SVs used in the fix */
+  uint8       u_TotalBdsSvs;            /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
+"""
+
+def name_to_camelcase(nam):
+  ret = []
+  i = 0
+  while i < len(nam):
+    if nam[i] == "_":
+      ret.append(nam[i+1].upper())
+      i += 2
+    else:
+      ret.append(nam[i])
+      i += 1
+  return ''.join(ret)
+
+def parse_struct(ss):
+  st = "<"
+  nams = []
+  for l in ss.strip().split("\n"):
+    typ, nam = l.split(";")[0].split()
+    #print(typ, nam)
+    if typ == "float" or '_Flt' in nam:
+      st += "f"
+    elif typ == "double" or '_Dbl' in nam:
+      st += "d"
+    elif typ in ["uint8", "uint8_t"]:
+      st += "B"
+    elif typ in ["int8", "int8_t"]:
+      st += "b"
+    elif typ in ["uint32", "uint32_t"]:
+      st += "I"
+    elif typ in ["int32", "int32_t"]:
+      st += "i"
+    elif typ in ["uint16", "uint16_t"]:
+      st += "H"
+    elif typ in ["int16", "int16_t"]:
+      st += "h"
+    elif typ == "uint64":
+      st += "Q"
+    else:
+      print("unknown type", typ)
+      assert False
+    if '[' in nam:
+      cnt = int(nam.split("[")[1].split("]")[0])
+      st += st[-1]*(cnt-1)
+      for i in range(cnt):
+        nams.append("%s[%d]" % (nam.split("[")[0], i))
+    else:
+      nams.append(nam)
+  return st, nams
+
+def dict_unpacker(ss, camelcase = False):
+  st, nams = parse_struct(ss)
+  if camelcase:
+    nams = [name_to_camelcase(x) for x in nams]
+  sz = calcsize(st)
+  return lambda x: dict(zip(nams, unpack_from(st, x))), sz

selfdrive/test/process_replay/compare_logs.py

@@ -25,7 +25,7 @@ def save_log(dest, log_msgs, compress=True):
     dat = bz2.compress(dat)
 
   with open(dest, "wb") as f:
-   f.write(dat)
+    f.write(dat)
 
 
 def remove_ignored_fields(msg, ignore):

selfdrive/test/process_replay/process_replay.py

@@ -231,7 +231,7 @@ def ublox_rcv_callback(msg):
   elif (msg_class, msg_id) in {(2, 1 * 16 + 5), (10, 9)}:
     return ["ubloxGnss"]
   else:
-     return []
+    return []
 
 
 CONFIGS = [

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-5bf6cd4a8d3c96eb333ae58d81af3907ac8b2804
+81ec7708ef7582ab950f9af4810d403fce292eb1

selfdrive/test/process_replay/regen.py

@@ -61,10 +61,10 @@ def replay_manager_state(s, msgs):
   rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
 
   while True:
-      new_m = messaging.new_message('managerState')
-      new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes]
-      pm.send(s, new_m)
-      rk.keep_time()
+    new_m = messaging.new_message('managerState')
+    new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes]
+    pm.send(s, new_m)
+    rk.keep_time()
 
 
 def replay_device_state(s, msgs):

selfdrive/test/test_onroad.py

@@ -56,7 +56,7 @@ if TICI:
   PROCS.update({
     "./loggerd": 70.0,
     "selfdrive.controls.controlsd": 31.0,
-    "./camerad": 36.8,
+    "./camerad": 41.0,
     "./_ui": 33.0,
     "selfdrive.controls.plannerd": 11.7,
     "./_dmonitoringmodeld": 10.0,
@@ -106,6 +106,7 @@ def check_cpu_usage(first_proc, last_proc):
   r = True
   dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9
   for proc_name, normal_cpu_usage in PROCS.items():
+    err = ""
     first, last = None, None
     try:
       first = [p for p in first_proc.procLog.procs if proc_name in p.cmdline][0]
@@ -115,15 +116,16 @@ def check_cpu_usage(first_proc, last_proc):
       if cpu_usage > max(normal_cpu_usage * 1.15, normal_cpu_usage + 5.0):
         # cpu usage is high while playing sounds
         if not (proc_name == "./_soundd" and cpu_usage < 65.):
-          result += f"Warning {proc_name} using more CPU than normal\n"
-          r = False
+          err = "using more CPU than normal"
       elif cpu_usage < min(normal_cpu_usage * 0.65, max(normal_cpu_usage - 1.0, 0.0)):
-        result += f"Warning {proc_name} using less CPU than normal\n"
-        r = False
-      result += f"{proc_name.ljust(35)}  {cpu_usage:.2f}%\n"
+        err = "using less CPU than normal"
     except IndexError:
-      result += f"{proc_name.ljust(35)}  NO METRICS FOUND {first=} {last=}\n"
+      err = f"NO METRICS FOUND {first=} {last=}\n"
+
+    result += f"{proc_name.ljust(35)}  {cpu_usage:5.2f}% ({normal_cpu_usage:5.2f}%) {err}\n"
+    if len(err) > 0:
       r = False
+
   result += "------------------------------------------------\n"
   print(result)
   return r

selfdrive/tombstoned.py

@@ -153,9 +153,9 @@ def report_tombstone_apport(fn):
     # Try to find first entry in openpilot, fall back to first line
     for line in stacktrace_s:
       if "at selfdrive/" in line:
-          crash_function = line
-          found = True
-          break
+        crash_function = line
+        found = True
+        break
 
     if not found:
       crash_function = stacktrace_s[1]

selfdrive/ui/qt/offroad/onboarding.cc

@@ -23,7 +23,7 @@ void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) {
 
   if (boundingRect[currentIndex].contains(e->x(), e->y())) {
     if (currentIndex == 9) {
-      const QRect yes = QRect(692, 842, 492, 148);
+      const QRect yes = QRect(707, 804, 531, 164);
       Params().putBool("RecordFront", yes.contains(e->x(), e->y()));
     }
     currentIndex += 1;

selfdrive/ui/qt/offroad/onboarding.h

@@ -25,52 +25,52 @@ private:
   int currentIndex = 0;
 
   // Bounding boxes for each training guide step
-  const QRect continueBtnStandard = {1610, 0, 310, 1080};
+  const QRect continueBtnStandard = {1620, 0, 300, 1080};
   QVector<QRect> boundingRectStandard {
-    QRect(650, 710, 720, 190),
+    QRect(112, 804, 619, 166),
     continueBtnStandard,
     continueBtnStandard,
-    QRect(1442, 565, 230, 310),
-    QRect(1515, 562, 133, 60),
+    QRect(1476, 565, 253, 308),
+    QRect(1501, 529, 184, 108),
     continueBtnStandard,
-    QRect(1580, 630, 215, 130),
-    QRect(1210, 0, 485, 590),
-    QRect(1460, 400, 375, 210),
-    QRect(166, 842, 1019, 148),
-    QRect(1460, 210, 300, 310),
+    QRect(1613, 665, 178, 153),
+    QRect(1220, 0, 420, 730),
+    QRect(1335, 499, 440, 147),
+    QRect(112, 820, 996, 148),
+    QRect(1412, 199, 316, 333),
     continueBtnStandard,
-    QRect(1375, 80, 545, 1000),
+    QRect(1237, 63, 683, 1017),
     continueBtnStandard,
-    QRect(1610, 130, 280, 800),
-    QRect(1385, 485, 400, 270),
+    QRect(1455, 110, 313, 860),
+    QRect(1253, 519, 383, 228),
     continueBtnStandard,
     continueBtnStandard,
-    QRect(1036, 769, 718, 189),
-    QRect(201, 769, 718, 189),
+    QRect(630, 804, 626, 164),
+    QRect(108, 804, 426, 164),
   };
 
-  const QRect continueBtnWide = {1850, 0, 310, 1080};
+  const QRect continueBtnWide = {1840, 0, 320, 1080};
   QVector<QRect> boundingRectWide {
-    QRect(654, 721, 718, 189),
+    QRect(112, 804, 618, 164),
     continueBtnWide,
     continueBtnWide,
-    QRect(1690, 570, 165, 300),
-    QRect(1690, 560, 133, 60),
+    QRect(1641, 558, 210, 313),
+    QRect(1662, 528, 184, 108),
     continueBtnWide,
-    QRect(1820, 630, 180, 155),
-    QRect(1360, 0, 460, 620),
-    QRect(1570, 400, 375, 215),
-    QRect(167, 842, 1018, 148),
-    QRect(1610, 210, 295, 310),
+    QRect(1814, 621, 211, 170),
+    QRect(1350, 0, 497, 755),
+    QRect(1553, 516, 406, 112),
+    QRect(112, 804, 1126, 164),
+    QRect(1598, 199, 316, 333),
     continueBtnWide,
-    QRect(1555, 90, 610, 990),
+    QRect(1364, 90, 796, 990),
     continueBtnWide,
-    QRect(1600, 140, 280, 790),
-    QRect(1385, 490, 750, 270),
+    QRect(1593, 114, 318, 853),
+    QRect(1379, 511, 391, 243),
     continueBtnWide,
     continueBtnWide,
-    QRect(1138, 755, 718, 189),
-    QRect(303, 755, 718, 189),
+    QRect(630, 804, 626, 164),
+    QRect(108, 804, 426, 164),
   };
 
   QString img_path;

selfdrive/ui/qt/onroad.cc

@@ -267,6 +267,11 @@ void OnroadHud::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, flo
 }
 
 // NvgWindow
+
+NvgWindow::NvgWindow(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraViewWidget("camerad", type, true, parent) {
+
+}
+
 void NvgWindow::initializeGL() {
   CameraViewWidget::initializeGL();
   qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
@@ -376,9 +381,9 @@ void NvgWindow::paintGL() {
 
   double cur_draw_t = millis_since_boot();
   double dt = cur_draw_t - prev_draw_t;
-  if (dt > 66) {
-    // warn on sub 15fps
-    LOGW("slow frame time: %.2f", dt);
+  double fps = fps_filter.update(1. / dt * 1000);
+  if (fps < 15) {
+    LOGW("slow frame rate: %.2f fps", fps);
   }
   prev_draw_t = cur_draw_t;
 }

selfdrive/ui/qt/onroad.h

@@ -3,6 +3,7 @@
 #include <QStackedLayout>
 #include <QWidget>
 
+#include "selfdrive/common/util.h"
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 #include "selfdrive/ui/ui.h"
 
@@ -66,7 +67,7 @@ class NvgWindow : public CameraViewWidget {
   Q_OBJECT
 
 public:
-  explicit NvgWindow(VisionStreamType type, QWidget* parent = 0) : CameraViewWidget("camerad", type, true, parent) {}
+  explicit NvgWindow(VisionStreamType type, QWidget* parent = 0);
 
 protected:
   void paintGL() override;
@@ -77,7 +78,9 @@ protected:
   void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd);
   inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }
   inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); }
+
   double prev_draw_t = 0;
+  FirstOrderFilter fps_filter;
 };
 
 // container for all onroad widgets

selfdrive/ui/qt/widgets/controls.h

@@ -101,7 +101,7 @@ public:
     QObject::connect(&toggle, &Toggle::stateChanged, this, &ToggleControl::toggleFlipped);
   }
 
-  void setEnabled(bool enabled) { toggle.setEnabled(enabled); }
+  void setEnabled(bool enabled) { toggle.setEnabled(enabled); toggle.update(); }
 
 signals:
   void toggleFlipped(bool state);

tools/lib/auth.py

@@ -86,13 +86,13 @@ def auth_redirect_link(method):
     })
     return 'https://github.com/login/oauth/authorize?' + urlencode(params)
   elif method == 'apple':
-      params.update({
-        'client_id': 'ai.comma.login',
-        'response_type': 'code',
-        'response_mode': 'form_post',
-        'scope': 'name email',
-      })
-      return 'https://appleid.apple.com/auth/authorize?' + urlencode(params)
+    params.update({
+      'client_id': 'ai.comma.login',
+      'response_type': 'code',
+      'response_mode': 'form_post',
+      'scope': 'name email',
+    })
+    return 'https://appleid.apple.com/auth/authorize?' + urlencode(params)
   else:
     raise NotImplementedError(f"no redirect implemented for method {method}")
 

tools/lib/tests/test_caching.py

@@ -66,4 +66,4 @@ class TestFileDownload(unittest.TestCase):
 
 
 if __name__ == "__main__":
-    unittest.main()
+  unittest.main()

tools/sim/bridge.py

@@ -48,6 +48,7 @@ class VehicleState:
     self.vel = carla.Vector3D()
     self.cruise_button = 0
     self.is_engaged = False
+    self.ignition = True
 
 
 def steer_rate_limit(old, new):
@@ -94,7 +95,7 @@ class Camerad:
     yuv_cl = cl_array.empty_like(rgb_cl)
     self.krnl(self.queue, (np.int32(self.Wdiv4), np.int32(self.Hdiv4)), None, rgb_cl.data, yuv_cl.data).wait()
     yuv = np.resize(yuv_cl.get(), np.int32(rgb.size / 2))
-    eof = self.frame_id * 0.05
+    eof = int(self.frame_id * 0.05 * 1e9)
 
     # TODO: remove RGB send once the last RGB vipc subscriber is removed
     self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_ROAD, img.tobytes(), self.frame_id, eof, eof)
@@ -126,13 +127,13 @@ def imu_callback(imu, vehicle_state):
   pm.send('sensorEvents', dat)
 
 
-def panda_state_function(exit_event: threading.Event):
+def panda_state_function(vs: VehicleState, exit_event: threading.Event):
   pm = messaging.PubMaster(['pandaStates'])
   while not exit_event.is_set():
     dat = messaging.new_message('pandaStates', 1)
     dat.valid = True
     dat.pandaStates[0] = {
-      'ignitionLine': True,
+      'ignitionLine': vs.ignition,
       'pandaType': "blackPanda",
       'controlsAllowed': True,
       'safetyModel': 'hondaNidec'
@@ -283,7 +284,7 @@ def bridge(q):
   # launch fake car threads
   threads = []
   exit_event = threading.Event()
-  threads.append(threading.Thread(target=panda_state_function, args=(exit_event,)))
+  threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, exit_event,)))
   threads.append(threading.Thread(target=peripheral_state_function, args=(exit_event,)))
   threads.append(threading.Thread(target=fake_driver_monitoring, args=(exit_event,)))
   threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, exit_event,)))
@@ -346,6 +347,8 @@ def bridge(q):
         elif m[1] == "cancel":
           cruise_button = CruiseButtons.CANCEL
           is_openpilot_engaged = False
+      elif m[0] == "ignition":
+        vehicle_state.ignition = not vehicle_state.ignition
       elif m[0] == "quit":
         break
 

tools/sim/lib/keyboard_ctrl.py

@@ -53,6 +53,8 @@ def keyboard_poll_thread(q: 'Queue[str]'):
       q.put("brake_%f" % 1.0)
     elif c == 'd':
       q.put("steer_%f" % -0.15)
+    elif c == 'i':
+      q.put("ignition")
     elif c == 'q':
       q.put("quit")
       break

tools/sim/lib/manual_ctrl.py

@@ -9,8 +9,8 @@ from typing import NoReturn
 # Iterate over the joystick devices.
 print('Available devices:')
 for fn in os.listdir('/dev/input'):
-    if fn.startswith('js'):
-        print(f'  /dev/input/{fn}')
+  if fn.startswith('js'):
+    print(f'  /dev/input/{fn}')
 
 # We'll store the states here.
 axis_states = {}
@@ -18,74 +18,74 @@ button_states = {}
 
 # 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',
+  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',
+  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',
+  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',
+  # XBox 360 controller uses these codes.
+  0x2c0 : 'dpad_left',
+  0x2c1 : 'dpad_right',
+  0x2c2 : 'dpad_up',
+  0x2c3 : 'dpad_down',
 }
 
 axis_map = []
@@ -118,18 +118,18 @@ def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
   ioctl(jsdev, 0x80406a32, buf)  # JSIOCGAXMAP
 
   for _axis in buf[:num_axes]:
-      axis_name = axis_names.get(_axis, f'unknown(0x{_axis:02x})')
-      axis_map.append(axis_name)
-      axis_states[axis_name] = 0.0
+    axis_name = axis_names.get(_axis, f'unknown(0x{_axis:02x})')
+    axis_map.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_map.append(btn_name)
-      button_states[btn_name] = 0
+    btn_name = button_names.get(btn, f'unknown(0x{btn:03x})')
+    button_map.append(btn_name)
+    button_states[btn_name] = 0
 
   print('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
   print('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))

tools/zookeeper/enable_and_wait.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-
 import os
 import sys
 import time
@@ -7,11 +6,11 @@ from socket import gethostbyname, gaierror
 from tools.zookeeper import Zookeeper
 
 def is_online(ip):
-    try:
-        addr = gethostbyname(ip)
-        return (os.system(f"ping -c 1 {addr} > /dev/null") == 0)
-    except gaierror:
-        return False
+  try:
+    addr = gethostbyname(ip)
+    return (os.system(f"ping -c 1 {addr} > /dev/null") == 0)
+  except gaierror:
+    return False
 
 if __name__ == "__main__":
   z = Zookeeper()