Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into SunnyLateral

# Conflicts:
#	opendbc_repo

launch_openpilot.sh

@@ -1,3 +1,20 @@
 #!/usr/bin/env bash
+set -euo pipefail
+IFS=$'\n\t'
+
+# On any failure, run the fallback launcher
+trap 'exec ./launch_chffrplus.sh' ERR
+C3_LAUNCH_SH="./sunnypilot/system/hardware/c3/launch_chffrplus.sh"
+
+MODEL="$(tr -d '\0' < "/sys/firmware/devicetree/base/model")"
+export MODEL
+
+if [ "$MODEL" = "comma tici" ]; then
+  # Force a failure if the launcher doesn't exist
+  [ -x "$C3_LAUNCH_SH" ] || false
+
+  # If it exists, run it
+  exec "$C3_LAUNCH_SH"
+fi
 
 exec ./launch_chffrplus.sh

selfdrive/controls/controlsd.py

@@ -95,6 +95,8 @@ class Controls(ControlsExt, ModelStateBase):
         self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
                                            torque_params.frictionCoefficientFiltered)
 
+        self.LaC.extension.update_limits()
+
       self.LaC.extension.update_model_v2(self.sm['modelV2'])
 
       self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay)

selfdrive/controls/lib/latcontrol_torque.py

@@ -35,7 +35,7 @@ class LatControlTorque(LatControl):
     self.update_limits()
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
 
-    self.extension = LatControlTorqueExt(self, CP, CP_SP)
+    self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
 
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     self.torque_params.latAccelFactor = latAccelFactor
@@ -73,12 +73,6 @@ class LatControlTorque(LatControl):
       ff = gravity_adjusted_lateral_accel
       ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
-      # Lateral acceleration torque controller extension updates
-      # Overrides stock ff and pid_log.error
-      ff, pid_log = self.extension.update(CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
-                                          desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
-                                          desired_curvature, actual_curvature)
-
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_lataccel = self.pid.update(pid_log.error,
                                       feedforward=ff,
@@ -86,6 +80,12 @@ class LatControlTorque(LatControl):
                                       freeze_integrator=freeze_integrator)
       output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
 
+      # Lateral acceleration torque controller extension updates
+      # Overrides pid_log.error and output_torque
+      pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+                                                     desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
+                                                     desired_curvature, actual_curvature, steer_limited_by_safety, output_torque)
+
       pid_log.active = True
       pid_log.p = float(self.pid.p)
       pid_log.i = float(self.pid.i)

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py

@@ -9,23 +9,28 @@ from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkL
 
 
 class LatControlTorqueExt(NeuralNetworkLateralControl):
-  def __init__(self, lac_torque, CP, CP_SP):
-    super().__init__(lac_torque, CP, CP_SP)
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    super().__init__(lac_torque, CP, CP_SP, CI)
 
-  def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+  def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
              desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
-             desired_curvature, actual_curvature):
+             desired_curvature, actual_curvature, steer_limited_by_safety, output_torque):
     self._ff = ff
+    self._pid = pid
     self._pid_log = pid_log
     self._setpoint = setpoint
     self._measurement = measurement
+    self._roll_compensation = roll_compensation
     self._lateral_accel_deadzone = lateral_accel_deadzone
     self._desired_lateral_accel = desired_lateral_accel
     self._actual_lateral_accel = actual_lateral_accel
     self._desired_curvature = desired_curvature
     self._actual_curvature = actual_curvature
+    self._gravity_adjusted_lateral_accel = gravity_adjusted_lateral_accel
+    self._steer_limited_by_safety = steer_limited_by_safety
+    self._output_torque = output_torque
 
     self.update_calculations(CS, VM, desired_lateral_accel)
     self.update_neural_network_feedforward(CS, params, calibrated_pose)
 
-    return self._ff, self._pid_log
+    return self._pid_log, self._output_torque

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py

@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
 import math
 import numpy as np
 
+from openpilot.common.pid import PIDController
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
@@ -43,9 +44,10 @@ def get_lookahead_value(future_vals, current_val):
 
 
 class LatControlTorqueExtBase:
-  def __init__(self, lac_torque, CP, CP_SP):
+  def __init__(self, lac_torque, CP, CP_SP, CI):
     self.model_v2 = None
     self.model_valid = False
+    self.lac_torque = lac_torque
     self.torque_params = lac_torque.torque_params
 
     self.actual_lateral_jerk: float = 0.0
@@ -53,17 +55,22 @@ class LatControlTorqueExtBase:
     self.lateral_jerk_measurement: float = 0.0
     self.lookahead_lateral_jerk: float = 0.0
 
-    self.torque_from_lateral_accel = lac_torque.torque_from_lateral_accel
+    self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space()
 
     self._ff = 0.0
+    self._pid = PIDController(0.0, 0.0, k_f=0.0)
     self._pid_log = None
     self._setpoint = 0.0
     self._measurement = 0.0
+    self._roll_compensation = 0.0
     self._lateral_accel_deadzone = 0.0
     self._desired_lateral_accel = 0.0
     self._actual_lateral_accel = 0.0
     self._desired_curvature = 0.0
     self._actual_curvature = 0.0
+    self._gravity_adjusted_lateral_accel = 0.0
+    self._steer_limited_by_safety = False
+    self._output_torque = 0.0
 
     # twilsonco's Lateral Neural Network Feedforward
     # Instantaneous lateral jerk changes very rapidly, making it not useful on its own,

sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py

@@ -9,6 +9,8 @@ import math
 import numpy as np
 
 from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from opendbc.sunnypilot.car.interfaces import LatControlInputs
+from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.selfdrive.modeld.constants import ModelConstants
@@ -30,8 +32,8 @@ def roll_pitch_adjust(roll, pitch):
 
 
 class NeuralNetworkLateralControl(LatControlTorqueExtBase):
-  def __init__(self, lac_torque, CP, CP_SP):
-    super().__init__(lac_torque, CP, CP_SP)
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    super().__init__(lac_torque, CP, CP_SP, CI)
     self.params = Params()
     self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
     self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH
@@ -57,14 +59,44 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
     self.error_deque = deque(maxlen=history_check_frames[0])
     self.past_future_len = len(self.past_times) + len(self.nn_future_times)
 
+  @property
+  def _nnlc_enabled(self):
+    return self.enabled and self.model_valid and self.has_nn_model
+
+  def update_limits(self):
+    if not self._nnlc_enabled:
+      return
+
+    self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
   def update_lateral_lag(self, lag):
     super().update_lateral_lag(lag)
     self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
 
+  def update_feedforward_torque_space(self, CS):
+    torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo),
+                                                                          self.torque_params, gravity_adjusted=False)
+    torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo),
+                                                                             self.torque_params, gravity_adjusted=False)
+    self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+    self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
+                                                                               CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True)
+    self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
+                                             FRICTION_THRESHOLD, self.torque_params)
+
+  def update_output_torque(self, CS):
+    freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+    self._output_torque = self._pid.update(self._pid_log.error,
+                                           feedforward=self._ff,
+                                           speed=CS.vEgo,
+                                           freeze_integrator=freeze_integrator)
+
   def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
-    if not self.enabled or not self.model_valid or not self.has_nn_model:
+    if not self._nnlc_enabled:
       return
 
+    self.update_feedforward_torque_space(CS)
+
     low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
     self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature
     self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature
@@ -128,3 +160,5 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
     # apply friction override for cars with low NN friction response
     if self.model.friction_override:
       self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    self.update_output_torque(CS)

sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py

@@ -3,6 +3,7 @@ from parameterized import parameterized
 
 from cereal import car, log, messaging
 from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM
 from opendbc.car.honda.values import CAR as HONDA
 from opendbc.car.hyundai.values import CAR as HYUNDAI
 from opendbc.car.toyota.values import CAR as TOYOTA
@@ -41,7 +42,7 @@ def generate_modelV2():
 
 class TestNeuralNetworkLateralControl:
 
-  @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
+  @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV])
   def test_saturation(self, car_name):
     params = Params()
     params.put_bool("NeuralNetworkLateralControl", True)
@@ -57,6 +58,7 @@ class TestNeuralNetworkLateralControl:
     VM = VehicleModel(CP)
 
     controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI)
+    torque_params = CP.lateralTuning.torque
 
     CS = car.CarState.new_message()
     CS.vEgo = 30
@@ -77,17 +79,23 @@ class TestNeuralNetworkLateralControl:
     for _ in range(1000):
       controller.extension.update_model_v2(model_v2)
       controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
       _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True)
     assert lac_log.saturated
 
     for _ in range(1000):
       controller.extension.update_model_v2(model_v2)
       controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
       _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False)
     assert not lac_log.saturated
 
     for _ in range(1000):
       controller.extension.update_model_v2(model_v2)
       controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
       _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False)
     assert lac_log.saturated

sunnypilot/system/hardware/c3/README.md

@@ -0,0 +1,3 @@
+# C3 specific hardware code
+
+`c3` is known as `tici` and comma three by comma. Not to confuse it with `c3x` which is known as `tizi`.

sunnypilot/system/hardware/c3/agnos.json

@@ -0,0 +1,84 @@
+[
+  {
+    "name": "xbl",
+    "url": "https://commadist.azureedge.net/agnosupdate/xbl-effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b.img.xz",
+    "hash": "effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b",
+    "hash_raw": "effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b",
+    "size": 3282256,
+    "sparse": false,
+    "full_check": true,
+    "has_ab": true,
+    "ondevice_hash": "ed61a650bea0c56652dd0fc68465d8fc722a4e6489dc8f257630c42c6adcdc89"
+  },
+  {
+    "name": "xbl_config",
+    "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c.img.xz",
+    "hash": "63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c",
+    "hash_raw": "63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c",
+    "size": 98124,
+    "sparse": false,
+    "full_check": true,
+    "has_ab": true,
+    "ondevice_hash": "b12801ffaa81e58e3cef914488d3b447e35483ba549b28c6cd9deb4814c3265f"
+  },
+  {
+    "name": "abl",
+    "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
+    "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
+    "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
+    "size": 274432,
+    "sparse": false,
+    "full_check": true,
+    "has_ab": true,
+    "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
+  },
+  {
+    "name": "aop",
+    "url": "https://commadist.azureedge.net/agnosupdate/aop-21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9.img.xz",
+    "hash": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9",
+    "hash_raw": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9",
+    "size": 184364,
+    "sparse": false,
+    "full_check": true,
+    "has_ab": true,
+    "ondevice_hash": "c1be2f4aac5b3af49b904b027faec418d05efd7bd5144eb4fdfcba602bcf2180"
+  },
+  {
+    "name": "devcfg",
+    "url": "https://commadist.azureedge.net/agnosupdate/devcfg-d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620.img.xz",
+    "hash": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620",
+    "hash_raw": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620",
+    "size": 40336,
+    "sparse": false,
+    "full_check": true,
+    "has_ab": true,
+    "ondevice_hash": "17b229668b20305ff8fa3cd5f94716a3aaa1e5bf9d1c24117eff7f2f81ae719f"
+  },
+  {
+    "name": "boot",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
+    "hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
+    "hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
+    "size": 18515968,
+    "sparse": false,
+    "full_check": true,
+    "has_ab": true,
+    "ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
+  },
+  {
+    "name": "system",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
+    "hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
+    "hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
+    "size": 5368709120,
+    "sparse": true,
+    "full_check": false,
+    "has_ab": true,
+    "ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
+    "alt": {
+      "hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
+      "size": 5368709120
+    }
+  }
+]

sunnypilot/system/hardware/c3/launch_chffrplus.sh

@@ -0,0 +1,92 @@
+#!/usr/bin/env bash
+
+SP_C3_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
+DIR="$( cd "$SP_C3_DIR/../../../.." >/dev/null 2>&1 && pwd )"
+
+source "$SP_C3_DIR/launch_env.sh"
+
+function agnos_init {
+  # TODO: move this to agnos
+  sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta
+
+  # set success flag for current boot slot
+  sudo abctl --set_success
+
+  # TODO: do this without udev in AGNOS
+  # udev does this, but sometimes we startup faster
+  sudo chgrp gpu /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0
+  sudo chmod 660 /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0
+
+
+  if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then
+    AGNOS_PY="$DIR/system/hardware/tici/agnos.py"
+    MANIFEST="$SP_C3_DIR/agnos.json"
+    if $AGNOS_PY --verify $MANIFEST; then
+      sudo reboot
+    fi
+    $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST
+  fi
+}
+
+function launch {
+  # Remove orphaned git lock if it exists on boot
+  [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock
+
+  # Check to see if there's a valid overlay-based update available. Conditions
+  # are as follows:
+  #
+  # 1. The DIR init file has to exist, with a newer modtime than anything in
+  #    the DIR Git repo. This checks for local development work or the user
+  #    switching branches/forks, which should not be overwritten.
+  # 2. The FINALIZED consistent file has to exist, indicating there's an update
+  #    that completed successfully and synced to disk.
+
+  if [ -f "${DIR}/.overlay_init" ]; then
+    find ${DIR}/.git -newer ${DIR}/.overlay_init | grep -q '.' 2> /dev/null
+    if [ $? -eq 0 ]; then
+      echo "${DIR} has been modified, skipping overlay update installation"
+    else
+      if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then
+        if [ ! -d /data/safe_staging/old_openpilot ]; then
+          echo "Valid overlay update found, installing"
+          LAUNCHER_LOCATION="${BASH_SOURCE[0]}"
+
+          mv $DIR /data/safe_staging/old_openpilot
+          mv "${STAGING_ROOT}/finalized" $DIR
+          cd $DIR
+
+          echo "Restarting launch script ${LAUNCHER_LOCATION}"
+          unset AGNOS_VERSION
+          exec "${LAUNCHER_LOCATION}"
+        else
+          echo "openpilot backup found, not updating"
+          # TODO: restore backup? This means the updater didn't start after swapping
+        fi
+      fi
+    fi
+  fi
+
+  # handle pythonpath
+  ln -sfn $(pwd) /data/pythonpath
+  export PYTHONPATH="$PWD"
+
+  # hardware specific init
+  if [ -f /AGNOS ]; then
+    agnos_init
+  fi
+
+  # write tmux scrollback to a file
+  tmux capture-pane -pq -S-1000 > /tmp/launch_log
+
+  # start manager
+  cd $DIR/system/manager
+  if [ ! -f $DIR/prebuilt ]; then
+    ./build.py
+  fi
+  ./manager.py
+
+  # if broken, keep on screen error
+  while true; do sleep 1; done
+}
+
+launch

sunnypilot/system/hardware/c3/launch_env.sh

@@ -0,0 +1,11 @@
+#!/usr/bin/env bash
+
+export OMP_NUM_THREADS=1
+export MKL_NUM_THREADS=1
+export NUMEXPR_NUM_THREADS=1
+export OPENBLAS_NUM_THREADS=1
+export VECLIB_MAXIMUM_THREADS=1
+
+if [ -z "$AGNOS_VERSION" ]; then
+  export AGNOS_VERSION="12.8"
+fi