diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 9e6b96ba59..d5aa532490 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f9ba54bfa7..294722c06a 100755 --- a/selfdrive/controls/controlsd.py +++ b/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'] diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index dcf0003430..6bbce95bbc 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/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. diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 1b249a3d1a..7bd0abc115 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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): diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 1f1199565e..eb8a1ed5f9 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3aef57baca..613824a4ed 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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, diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 299652ff18..aee020c020 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/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 diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 4ebfc10940..6866bfb7d8 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/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") diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc similarity index 96% rename from selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.cc rename to selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc index d178c59b07..fd5d2fd5c8 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.cc +++ b/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" diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h similarity index 100% rename from selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot/mads_settings.h rename to selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc similarity index 87% rename from selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc rename to selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index 82d07febb8..e604b03ba9 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.cc +++ b/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); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h similarity index 83% rename from selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h rename to selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h index e955995b34..7cc4b6f11d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h @@ -11,16 +11,16 @@ #include #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; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc index a27561edf6..65e9744ed9 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc +++ b/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); diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py new file mode 100644 index 0000000000..1166cb32d6 --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py new file mode 100644 index 0000000000..48d98501b2 --- /dev/null +++ b/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