Merge branch 'master-new' into HKG-long-tune

selfdrive/car/tests/test_car_interfaces.py

@@ -42,7 +42,7 @@ class TestCarInterfaces:
     car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
                                          experimental_long=args['experimental_long'], docs=False)
     car_params_sp = CarInterface.get_params_sp(car_params, car_name, args['fingerprints'], args['car_fw'],
-                                                           experimental_long=args['experimental_long'], docs=False)
+                                               experimental_long=args['experimental_long'], docs=False)
     car_params = car_params.as_reader()
     car_interface = CarInterface(car_params, car_params_sp)
     assert car_params
@@ -100,8 +100,8 @@ class TestCarInterfaces:
     #  hypothesis also slows down significantly with just one more message draw
     LongControl(car_params)
     if car_params.steerControlType == CarParams.SteerControlType.angle:
-      LatControlAngle(car_params, car_interface)
+      LatControlAngle(car_params, car_params_sp, car_interface)
     elif car_params.lateralTuning.which() == 'pid':
-      LatControlPID(car_params, car_interface)
+      LatControlPID(car_params, car_params_sp, car_interface)
     elif car_params.lateralTuning.which() == 'torque':
-      LatControlTorque(car_params, car_interface)
+      LatControlTorque(car_params, car_params_sp, car_interface)

selfdrive/controls/controlsd.py

@@ -55,11 +55,11 @@ class Controls:
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      self.LaC = LatControlAngle(self.CP, self.CI)
+      self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
-      self.LaC = LatControlPID(self.CP, self.CI)
+      self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
     elif self.CP.lateralTuning.which() == 'torque':
-      self.LaC = LatControlTorque(self.CP, self.CI)
+      self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI)
 
   def update(self):
     self.sm.update(15)
@@ -85,6 +85,8 @@ class Controls:
         self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
                                            torque_params.frictionCoefficientFiltered)
 
+      self.LaC.extension.update_model_v2(self.sm['modelV2'])
+
     long_plan = self.sm['longitudinalPlan']
     model_v2 = self.sm['modelV2']
 

selfdrive/controls/lib/latcontrol.py

@@ -7,7 +7,7 @@ MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
 
 
 class LatControl(ABC):
-  def __init__(self, CP, CI):
+  def __init__(self, CP, CP_SP, CI):
     self.sat_count_rate = 1.0 * DT_CTRL
     self.sat_limit = CP.steerLimitTimer
     self.sat_count = 0.

selfdrive/controls/lib/latcontrol_angle.py

@@ -7,8 +7,8 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
 
 class LatControlAngle(LatControl):
-  def __init__(self, CP, CI):
-    super().__init__(CP, CI)
+  def __init__(self, CP, CP_SP, CI):
+    super().__init__(CP, CP_SP, CI)
     self.sat_check_min_speed = 5.
 
   def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):

selfdrive/controls/lib/latcontrol_pid.py

@@ -6,8 +6,8 @@ from openpilot.common.pid import PIDController
 
 
 class LatControlPID(LatControl):
-  def __init__(self, CP, CI):
-    super().__init__(CP, CI)
+  def __init__(self, CP, CP_SP, CI):
+    super().__init__(CP, CP_SP, CI)
     self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                              (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
                              k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)

selfdrive/controls/lib/latcontrol_torque.py

@@ -7,6 +7,8 @@ from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.common.pid import PIDController
 
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt
+
 # At higher speeds (25+mph) we can assume:
 # Lateral acceleration achieved by a specific car correlates to
 # torque applied to the steering rack. It does not correlate to
@@ -23,8 +25,8 @@ LOW_SPEED_Y = [15, 13, 10, 5]
 
 
 class LatControlTorque(LatControl):
-  def __init__(self, CP, CI):
-    super().__init__(CP, CI)
+  def __init__(self, CP, CP_SP, CI):
+    super().__init__(CP, CP_SP, CI)
     self.torque_params = CP.lateralTuning.torque.as_builder()
     self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
                              k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
@@ -32,6 +34,8 @@ class LatControlTorque(LatControl):
     self.use_steering_angle = self.torque_params.useSteeringAngle
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
 
+    self.extension = LatControlTorqueExt(self, CP, CP_SP)
+
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     self.torque_params.latAccelFactor = latAccelFactor
     self.torque_params.latAccelOffset = latAccelOffset
@@ -73,6 +77,11 @@ class LatControlTorque(LatControl):
                                           desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
                                           gravity_adjusted=True)
 
+      # 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)
+
       freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
       output_torque = self.pid.update(pid_log.error,
                                       feedforward=ff,

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -6,6 +6,7 @@ from opendbc.car.honda.values import CAR as HONDA
 from opendbc.car.toyota.values import CAR as TOYOTA
 from opendbc.car.nissan.values import CAR as NISSAN
 from opendbc.car.vehicle_model import VehicleModel
+from openpilot.selfdrive.car.helpers import convert_to_capnp
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
@@ -21,9 +22,10 @@ class TestLatControl:
     CP = CarInterface.get_non_essential_params(car_name)
     CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
     CI = CarInterface(CP, CP_SP)
+    CP_SP = convert_to_capnp(CP_SP)
     VM = VehicleModel(CP)
 
-    controller = controller(CP.as_reader(), CI)
+    controller = controller(CP.as_reader(), CP_SP.as_reader(), CI)
 
     CS = car.CarState.new_message()
     CS.vEgo = 30

selfdrive/ui/sunnypilot/SConscript

@@ -20,11 +20,11 @@ qt_src = [
   "sunnypilot/qt/home.cc",
   "sunnypilot/qt/offroad/offroad_home.cc",
   "sunnypilot/qt/offroad/settings/device_panel.cc",
+  "sunnypilot/qt/offroad/settings/lateral_panel.cc",
   "sunnypilot/qt/offroad/settings/settings.cc",
   "sunnypilot/qt/offroad/settings/software_panel.cc",
   "sunnypilot/qt/offroad/settings/sunnylink_panel.cc",
   "sunnypilot/qt/offroad/settings/sunnylink/sponsor_widget.cc",
-  "sunnypilot/qt/offroad/settings/sunnypilot_panel.cc",
   "sunnypilot/qt/offroad/settings/trips_panel.cc",
   "sunnypilot/qt/offroad/settings/vehicle_panel.cc",
   "sunnypilot/qt/onroad/annotated_camera.cc",
@@ -34,8 +34,8 @@ qt_src = [
   "sunnypilot/qt/onroad/onroad_home.cc",
 ]
 
-sunnypilot_panel_qt_src = [
-  "sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.cc",
+lateral_panel_qt_src = [
+  "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
 ]
 
 network_src = [
@@ -50,7 +50,7 @@ vehicle_panel_qt_src = [
 ]
 
 sp_widgets_src = widgets_src + network_src
-sp_qt_src = qt_src + sunnypilot_panel_qt_src + vehicle_panel_qt_src
+sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src
 sp_qt_util = qt_util
 
 Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util")

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc

@@ -5,7 +5,7 @@
  * See the LICENSE.md file in the root directory for more details.
  */
 
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
 
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc

@@ -5,12 +5,12 @@
  * See the LICENSE.md file in the root directory for more details.
  */
 
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h"
 
 #include "common/util.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 
-SunnypilotPanel::SunnypilotPanel(SettingsWindowSP *parent) : QFrame(parent) {
+LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   main_layout = new QStackedLayout(this);
   ListWidget *list = new ListWidget(this, false);
 
@@ -45,7 +45,7 @@ SunnypilotPanel::SunnypilotPanel(SettingsWindowSP *parent) : QFrame(parent) {
   toggleOffroadOnly = {
     madsToggle,
   };
-  QObject::connect(uiState(), &UIState::offroadTransition, this, &SunnypilotPanel::updateToggles);
+  QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles);
 
   sunnypilotScroller = new ScrollViewSP(list, this);
   vlayout->addWidget(sunnypilotScroller);
@@ -71,15 +71,15 @@ SunnypilotPanel::SunnypilotPanel(SettingsWindowSP *parent) : QFrame(parent) {
   main_layout->setCurrentWidget(sunnypilotScreen);
 }
 
-void SunnypilotPanel::showEvent(QShowEvent *event) {
+void LateralPanel::showEvent(QShowEvent *event) {
   updateToggles(offroad);
 }
 
-void SunnypilotPanel::hideEvent(QHideEvent *event) {
+void LateralPanel::hideEvent(QHideEvent *event) {
   main_layout->setCurrentWidget(sunnypilotScreen);
 }
 
-void SunnypilotPanel::updateToggles(bool _offroad) {
+void LateralPanel::updateToggles(bool _offroad) {
   for (auto *toggle : toggleOffroadOnly) {
     toggle->setEnabled(_offroad);
   }

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

@@ -11,16 +11,16 @@
 #include <string>
 
 #include "selfdrive/ui/sunnypilot/ui.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 
-class SunnypilotPanel : public QFrame {
+class LateralPanel : public QFrame {
   Q_OBJECT
 
 public:
-  explicit SunnypilotPanel(SettingsWindowSP *parent = nullptr);
+  explicit LateralPanel(SettingsWindowSP *parent = nullptr);
   void showEvent(QShowEvent *event) override;
   void hideEvent(QHideEvent* event) override;
 

selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc

@@ -15,7 +15,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_panel.h"
 
@@ -78,7 +78,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
     PanelInfo("   " + tr("sunnylink"), new SunnylinkPanel(this), "../assets/offroad/icon_wifi_strength_full.svg"),
     PanelInfo("   " + tr("Toggles"), toggles, "../../sunnypilot/selfdrive/assets/offroad/icon_toggle.png"),
     PanelInfo("   " + tr("Software"), new SoftwarePanelSP(this), "../../sunnypilot/selfdrive/assets/offroad/icon_software.png"),
-    PanelInfo("   " + tr("sunnypilot"), new SunnypilotPanel(this), "../assets/images/button_home.png"),
+    PanelInfo("   " + tr("Steering"), new LateralPanel(this), "../assets/images/button_home.png"),
     PanelInfo("   " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"),
     PanelInfo("   " + tr("Vehicle"), new VehiclePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"),
     PanelInfo("   " + tr("Firehose"), new FirehosePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_firehose.svg"),
@@ -117,7 +117,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
     nav_btns->addButton(btn);
     buttons_layout->addWidget(btn, 0, Qt::AlignLeft | Qt::AlignBottom);
 
-    const int lr_margin = (name != ("   " + tr("Network")) || (name != ("   " + tr("sunnypilot")))) ? 50 : 0;  // Network and sunnypilot panel handles its own margins
+    const int lr_margin = (name != ("   " + tr("Network"))) ? 50 : 0;  // Network panel handles its own margins
     panel->setContentsMargins(lr_margin, 25, lr_margin, 25);
 
     ScrollViewSP *panel_frame = new ScrollViewSP(panel, this);

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py

@@ -0,0 +1,26 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase
+
+
+class LatControlTorqueExt(LatControlTorqueExtBase):
+  def __init__(self, lac_torque, CP, CP_SP):
+    super().__init__(lac_torque, CP, CP_SP)
+
+  def update(self, 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):
+    self._ff = ff
+    self._pid_log = pid_log
+    self._setpoint = setpoint
+    self._measurement = measurement
+    self._lateral_accel_deadzone = lateral_accel_deadzone
+    self._desired_lateral_accel = desired_lateral_accel
+    self._actual_lateral_accel = actual_lateral_accel
+
+    self.update_calculations(CS, VM, desired_lateral_accel)
+
+    return self._ff, self._pid_log

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py

@@ -0,0 +1,121 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import math
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+LAT_PLAN_MIN_IDX = 5
+
+
+def get_predicted_lateral_jerk(lat_accels, t_diffs):
+  # compute finite difference between subsequent model_v2.acceleration.y values
+  # this is just two calls of np.diff followed by an element-wise division
+  lat_accel_diffs = np.diff(lat_accels)
+  lat_jerk = lat_accel_diffs / t_diffs
+  # return as python list
+  return lat_jerk.tolist()
+
+
+def sign(x):
+  return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0)
+
+
+def get_lookahead_value(future_vals, current_val):
+  if len(future_vals) == 0:
+    return current_val
+
+  same_sign_vals = [v for v in future_vals if sign(v) == sign(current_val)]
+
+  # if any future val has opposite sign of current val, return 0
+  if len(same_sign_vals) < len(future_vals):
+    return 0.0
+
+  # otherwise return the value with minimum absolute value
+  min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x))
+  return min_val
+
+
+class LatControlTorqueExtBase:
+  def __init__(self, lac_torque, CP, CP_SP):
+    self.model_v2 = None
+    self.model_valid = False
+    self.use_steering_angle = lac_torque.use_steering_angle
+
+    self.actual_lateral_jerk: float = 0.0
+    self.lateral_jerk_setpoint: float = 0.0
+    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_params = lac_torque.torque_params
+
+    self._ff = 0.0
+    self._pid_log = None
+    self._setpoint = 0.0
+    self._measurement = 0.0
+    self._lateral_accel_deadzone = 0.0
+    self._desired_lateral_accel = 0.0
+    self._actual_lateral_accel = 0.0
+
+    # twilsonco's Lateral Neural Network Feedforward
+    # Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
+    # however, we can "look ahead" to the future planned lateral jerk in order to gauge
+    # whether the current desired lateral jerk will persist into the future, i.e.
+    # whether it's "deliberate" or not. This allows us to simply ignore short-lived jerk.
+    # Note that LAT_PLAN_MIN_IDX is defined above and is used in order to prevent
+    # using a "future" value that is actually planned to occur before the "current" desired
+    # value, which is offset by the steerActuatorDelay.
+    # TODO-SP: Reevaluate lookahead v values that determines how low a desired lateral jerk signal needs to
+    #          persist in order to be used.
+    self.friction_look_ahead_v = [1.4, 2.0]  # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments
+    self.friction_look_ahead_bp = [9.0, 30.0]  # corresponding speeds in m/s in [0, ~40] in 1.0 increments
+
+    # Scaling the lateral acceleration "friction response" could be helpful for some.
+    # Increase for a stronger response, decrease for a weaker response.
+    self.lat_jerk_friction_factor = 0.4
+    self.lat_accel_friction_factor = 0.7  # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
+
+    # precompute time differences between ModelConstants.T_IDXS
+    self.t_diffs = np.diff(ModelConstants.T_IDXS)
+    self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3
+
+  def update_model_v2(self, model_v2):
+    self.model_v2 = model_v2
+    self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N
+
+  def update_friction_input(self, val_1, val_2):
+    _error = val_1 - val_2
+    _value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
+
+    return _value
+
+  def update_calculations(self, CS, VM, desired_lateral_accel):
+    self.actual_lateral_jerk = 0.0
+    self.lateral_jerk_setpoint = 0.0
+    self.lateral_jerk_measurement = 0.0
+    self.lookahead_lateral_jerk = 0.0
+
+    if self.use_steering_angle:
+      actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
+      self.actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
+
+    if self.model_valid:
+      # prepare "look-ahead" desired lateral jerk
+      lookahead = np.interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v)
+      friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16)
+      predicted_lateral_jerk = get_predicted_lateral_jerk(self.model_v2.acceleration.y, self.t_diffs)
+      desired_lateral_jerk = (np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS,
+                              self.model_v2.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time
+      self.lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
+      if not self.use_steering_angle or self.lookahead_lateral_jerk == 0.0:
+        self.lookahead_lateral_jerk = 0.0
+        self.actual_lateral_jerk = 0.0
+        self.lat_accel_friction_factor = 1.0
+      self.lateral_jerk_setpoint = self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
+      self.lateral_jerk_measurement = self.lat_jerk_friction_factor * self.actual_lateral_jerk