Merge pull request #1 from eFiniLan/master_dev

up to 2023-04-26 commits

Author: eFiniLan

.gitignore

@@ -83,3 +83,13 @@ build/
 !**/.gitkeep
 
 poetry.toml
+
+# rick - these are generated during compilation.
+selfdrive/camerad/camerad
+selfdrive/golden/
+selfdrive/legacy_modeld/_dmonitoringmodeld
+selfdrive/legacy_modeld/_modeld
+selfdrive/legacy_modeld/models/supercombo.thneed
+selfdrive/legacy_modeld/models/supercombo_badweights.thneed
+selfdrive/legacy_modeld/thneed/compile
+selfdrive/loggerd/bootlog

README.md

@@ -11,6 +11,13 @@ legacypilot is a side project that enables comma.ai EON and Comma Two devices to
 
 legacypilot combines the Nuclear Grade Model with the latest openpilot (almost) codebase to create a hybrid solution. With the legacypilot project, we have stripped out nearly 99% of the dragonpilot code.
 
+In summary, legacypilot is based on Openpilot 0.8.16 with the latest vehicle model support from the Openpilot master branch.
+
+Please note that this build is and will always be in the experimental phase and may not be suitable for use as a daily driver.
+
+I recommend using the openpilot [commatwo_master](https://github.com/commaai/openpilot/tree/commatwo_master) branch for your daily driving needs.
+
+Please see [README_openpilot.md](README_openpilot.md) for the original readme.
 
 ## Why use legacypilot
 
@@ -22,8 +29,9 @@ I have decided to make this side project open source for users who wish to:
 * Experience pure openpilot (without the modifications of dragonpilot)
 * Make their own EON/C2 fork without spending hundreds of hours reverting and testing code.
 
-By making this project open source, I hope to alleviate some of the frustration and complaints about not being able to access the source code. However, I still encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for the best and up-to-date openpilot experience.
+By making this project open source, I hope to alleviate some of the frustration and complaints about not being able to access the dragonpilot source code.
 
+I still encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for the best and up-to-date openpilot experience.
 
 ## Limitations
 
@@ -33,9 +41,3 @@ By making this project open source, I hope to alleviate some of the frustration
 * Navigation On Openpilot (NOO) is not supported, as it requires a newer driving model that is not currently available in legacypilot.
 * Services are not optimized for resource usage, and using all services may result in overheating issues.
 * Language files can only be generated in a PC due to missing Qt5 tools.
-
-In summary, legacypilot is based on Openpilot 0.8.16 with the latest vehicle model support from the Openpilot master branch.
-
-However, please note that this build is still in the experimental phase and may not be suitable for use as a daily driver. We recommend using the openpilot mastertwo branch for your daily driving needs.
-
-Please see README_openpilot.md for the original readme.

SConstruct

@@ -7,6 +7,8 @@ import numpy as np
 
 import SCons.Errors
 
+SCons.Warnings.warningAsException(True)
+
 TICI = os.path.isfile('/TICI')
 AGNOS = TICI
 
@@ -359,7 +361,9 @@ else:
     qt_libs = ['gui', 'adreno_utils']
   elif arch != "Darwin":
     qt_libs += ["GL"]
+qt_env['QT3DIR'] = qt_env['QTDIR']
 
+# compatibility for older SCons versions
 try:
   qt_env.Tool('qt3')
 except SCons.Errors.UserError:
@@ -491,7 +495,7 @@ SConscript(['system/sensord/SConscript'])
 SConscript(['selfdrive/ui/SConscript'])
 # SConscript(['selfdrive/navd/SConscript'])
 
-if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
+if (arch in ['x86_64', 'Darwin'] and Dir('#tools/cabana/').exists()) or GetOption('extras'):
   SConscript(['tools/replay/SConscript'])
   SConscript(['tools/cabana/SConscript'])
 

common/legacy_modeldata.h

@@ -5,8 +5,6 @@
 #include "system/hardware/hw.h"
 
 const int TRAJECTORY_SIZE = 33;
-const int LAT_MPC_N = 16;
-const int LON_MPC_N = 32;
 const float MIN_DRAW_DISTANCE = 10.0;
 const float MAX_DRAW_DISTANCE = 100.0;
 

docs/docker/Dockerfile

@@ -11,6 +11,7 @@ WORKDIR ${OPENPILOT_PATH}
 
 COPY SConstruct ${OPENPILOT_PATH}
 
+COPY ./body ${OPENPILOT_PATH}/body
 COPY ./third_party ${OPENPILOT_PATH}/third_party
 COPY ./site_scons ${OPENPILOT_PATH}/site_scons
 COPY ./laika ${OPENPILOT_PATH}/laika

opendbc

@@ -396,9 +396,9 @@ selfdrive/modeld/runners/run.h
 selfdrive/monitoring/dmonitoringd.py
 selfdrive/monitoring/driver_monitor.py
 
+selfdrive/navd/.gitignore
 selfdrive/navd/__init__.py
-selfdrive/navd/navd.py
-selfdrive/navd/helpers.py
+selfdrive/navd/**
 
 selfdrive/assets/.gitignore
 selfdrive/assets/assets.qrc

panda

@@ -2,7 +2,7 @@
 from common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_angle_limits
-from selfdrive.car.ford.fordcan import create_acc_command, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \
+from selfdrive.car.ford.fordcan import create_acc_msg, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \
   create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg
 from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams
 
@@ -76,7 +76,7 @@ def update(self, CC, CS, now_nanos):
         gas = -5.0
         decel = True
 
-      can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel))
+      can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, precharge_brake, decel))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)

release/files_common

@@ -97,7 +97,7 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float
   return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)
 
 
-def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool):
+def create_acc_msg(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool):
   """
   Creates a CAN message for the Ford ACC Command.
 
@@ -117,6 +117,40 @@ def create_acc_command(packer, long_active: bool, gas: float, accel: float, prec
   return packer.make_can_msg("ACCDATA", CANBUS.main, values)
 
 
+def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
+  """
+  Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status.
+
+  Stock functionality is maintained by passing through unmodified signals.
+
+  Frequency is 20Hz.
+  """
+
+  # Tja_D_Stat
+  if enabled:
+    if hud_control.leftLaneDepart:
+      status = 3  # ActiveInterventionLeft
+    elif hud_control.rightLaneDepart:
+      status = 4  # ActiveInterventionRight
+    else:
+      status = 2  # Active
+  elif main_on:
+    if hud_control.leftLaneDepart:
+      status = 5  # ActiveWarningLeft
+    elif hud_control.rightLaneDepart:
+      status = 6  # ActiveWarningRight
+    else:
+      status = 1  # Standby
+  else:
+    status = 0    # Off
+
+  values = {
+    **stock_values,
+    "Tja_D_Stat": status,
+  }
+  return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
+
+
 def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict):
   """
   Creates a CAN message for the Ford IPC IPMA/LKAS status.
@@ -168,40 +202,6 @@ def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool,
   return packer.make_can_msg("IPMA_Data", CANBUS.main, values)
 
 
-def create_acc_ui_msg(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
-  """
-  Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status.
-
-  Stock functionality is maintained by passing through unmodified signals.
-
-  Frequency is 20Hz.
-  """
-
-  # Tja_D_Stat
-  if enabled:
-    if hud_control.leftLaneDepart:
-      status = 3  # ActiveInterventionLeft
-    elif hud_control.rightLaneDepart:
-      status = 4  # ActiveInterventionRight
-    else:
-      status = 2  # Active
-  elif main_on:
-    if hud_control.leftLaneDepart:
-      status = 5  # ActiveWarningLeft
-    elif hud_control.rightLaneDepart:
-      status = 6  # ActiveWarningRight
-    else:
-      status = 1  # Standby
-  else:
-    status = 0    # Off
-
-  values = {
-    **stock_values,
-    "Tja_D_Stat": status,
-  }
-  return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
-
-
 def create_button_msg(packer, stock_values: dict, cancel=False, resume=False, tja_toggle=False,
                       bus: int = CANBUS.camera):
   """

selfdrive/car/ford/carcontroller.py

@@ -482,6 +482,7 @@ class SubaruCarInfo(CarInfo):
       b'\xa1 "\t\x01',
       b'\xa1  \x08\x02',
       b'\xa1 \x06\x02',
+      b'\xa1  \x07\x02',
       b'\xa1  \x08\x00',
       b'\xa1 "\t\x00',
     ],
@@ -493,6 +494,7 @@ class SubaruCarInfo(CarInfo):
     (Ecu.fwdCamera, 0x787, None): [
       b'\x00\x00eJ\x00\x1f@ \x19\x00',
       b'\000\000e\x80\000\037@ \031\000',
+      b'\x00\x00e\x9a\x00\x00\x00\x00\x00\x00',
       b'\x00\x00e\x9a\x00\x1f@ 1\x00',
       b'\x00\x00eJ\x00\x00\x00\x00\x00\x00',
     ],
@@ -502,6 +504,7 @@ class SubaruCarInfo(CarInfo):
       b'\xde"`0\a',
       b'\xf1\x82\xbc,\xa0q\a',
       b'\xf1\x82\xe3,\xa0@\x07',
+      b'\xe2"`0\x07',
       b'\xe2"`p\x07',
       b'\xf1\x82\xe2,\xa0@\x07',
       b'\xbc"`q\x07',
@@ -513,6 +516,7 @@ class SubaruCarInfo(CarInfo):
       b'\xa5\xfe\xf6@\x00',
       b'\xa7\x8e\xf40\x00',
       b'\xf1\x82\xa7\xf6D@\x00',
+      b'\xa7\xf6D@\x00',
       b'\xa7\xfe\xf4@\x00',
     ],
   },

selfdrive/car/ford/fordcan.py

@@ -719,7 +719,7 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
 
   EventName.calibrationIncomplete: {
     ET.PERMANENT: calibration_incomplete_alert,
-    ET.SOFT_DISABLE: soft_disable_alert("Calibration in Progress"),
+    ET.SOFT_DISABLE: soft_disable_alert("Device remount detected: recalibrating"),
     ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
   },
 

selfdrive/car/subaru/values.py

@@ -143,7 +143,7 @@ def update_status(self) -> None:
     # If spread is too high, assume mounting was changed and reset to last block.
     # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
     if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
-      self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)
+      self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
 
     write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
     if self.param_put and write_this_cycle:

selfdrive/controls/lib/events.py

@@ -180,6 +180,8 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
   fix.setDeviceStable(!this->device_fell);
   fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
+  // rick - disable it for now, compilation error, cereal looks fine, used in laikad
+//  fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
   this->device_fell = false;
 
   //fix.setGpsWeek(this->time.week);
@@ -573,6 +575,9 @@ void Localizer::time_check(double current_time) {
   if (std::isnan(this->last_reset_time)) {
     this->last_reset_time = current_time;
   }
+  if (std::isnan(this->first_valid_log_time)) {
+    this->first_valid_log_time = current_time;
+  }
   double filter_time = this->kf->get_filter_time();
   bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
   if (big_time_gap) {
@@ -765,6 +770,11 @@ int Localizer::locationd_thread() {
       bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
       #endif
 
+      // Log time to first fix
+      if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
+        this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
+      }
+
       MessageBuilder msg_builder;
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
       pm.send("liveLocationKalman", bytes.begin(), bytes.size());

selfdrive/locationd/calibrationd.py

@@ -79,6 +79,8 @@ class Localizer {
   double reset_tracker = 0.0;
   bool device_fell = false;
   bool gps_mode = false;
+  double first_valid_log_time = NAN;
+  double ttff = NAN;
   double last_gps_msg = 0;
   bool ublox_available = true;
   bool observation_timings_invalid = false;

selfdrive/locationd/locationd.cc

@@ -95,7 +95,7 @@ def test_calibration_auto_reset(self):
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
                          [1e-3, 1e-3, 1e-3])
-    self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
+    self.assertEqual(c.valid_blocks, 1)
     np.testing.assert_allclose(c.rpy, [0.0, 0.0, -0.05], atol=1e-2)
 
 if __name__ == "__main__":

selfdrive/locationd/locationd.h

@@ -20,7 +20,8 @@
 from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
 from system.swaglog import cloudlog, add_file_handler
 from system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \
-                              terms_version, training_version, is_tested_branch, is_release_branch
+                           get_normalized_origin, terms_version, training_version, \
+                           is_tested_branch, is_release_branch
 
 
 
@@ -92,7 +93,12 @@ def manager_init() -> None:
 
   # init logging
   sentry.init(sentry.SentryProject.SELFDRIVE)
-  cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=is_dirty(),
+  cloudlog.bind_global(dongle_id=dongle_id,
+                       version=get_version(),
+                       origin=get_normalized_origin(),
+                       branch=get_short_branch(),
+                       commit=get_commit(),
+                       dirty=is_dirty(),
                        device=HARDWARE.get_device_type())
 
 

selfdrive/locationd/test/test_calibrationd.py

@@ -1,21 +1,20 @@
 Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations')
 
-base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq',
-             'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"]
-
+map_env = qt_env.Clone()
+libs = ['qt_widgets', 'qt_util', 'qmapboxgl', common, messaging, cereal, visionipc, transformations,
+        'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"]
 if arch == 'larch64':
-  base_libs.append('EGL')
+  libs.append('EGL')
 
 if arch in ['larch64', 'x86_64']:
   if arch == 'x86_64':
-    rpath = [Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath]
-    qt_env["RPATH"] += rpath
+    rpath = Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath
+    map_env["RPATH"] += [rpath, ]
 
   style_path = File("style.json").abspath
-  qt_env['CXXFLAGS'].append(f'-DSTYLE_PATH=\\"{style_path}\\"')
-  qt_libs = ["qt_widgets", "qt_util", "qmapboxgl"] + base_libs
-
-  nav_src = ["main.cc", "map_renderer.cc"]
-  qt_env.Program("map_renderer", nav_src, LIBS=qt_libs + ['common', 'json11'])
+  map_env['CXXFLAGS'].append(f'-DSTYLE_PATH=\\"{style_path}\\"')
 
-  qt_env.SharedLibrary("map_renderer", ["map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging'])
+  map_env["RPATH"].append(Dir('.').abspath)
+  map_env["LIBPATH"].append(Dir('.').abspath)
+  maplib = map_env.SharedLibrary("maprender", ["map_renderer.cc"], LIBS=libs)
+  map_env.Program("map_renderer", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs)

selfdrive/manager/manager.py

@@ -279,7 +279,7 @@ def ublox_rcv_callback(msg, CP, cfg, fsm):
     fake_pubsubmaster=True,
     submaster_config={
       'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'],  # dcam is expected at 20 Hz
-      'ignore_alive': ['wideRoadCameraState'],  # TODO: Add to regen
+      'ignore_alive': [], 
     }
   ),
   ProcessConfig(

selfdrive/navd/SConscript

@@ -1 +1 @@
-28ac0bf6aa9142446917301071cf03797a724bf2
+627aa0f54e377d1f3954c58e37c0a15b555e20b3