Merge branch 'master-new' of https://github.com/infiniteCable2/openpilot into master-new

common/params_keys.h

@@ -37,6 +37,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"DoReboot", CLEAR_ON_MANAGER_START},
     {"DoShutdown", CLEAR_ON_MANAGER_START},
     {"DoUninstall", CLEAR_ON_MANAGER_START},
+    {"EnableCurvatureController", PERSISTENT},
     {"EnableScreenEvent", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"EnableSmoothSteer", PERSISTENT},
     {"EnableSpeedLimitControl", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -18,7 +18,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
-from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
+from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature, CURVATURE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW
@@ -58,6 +58,7 @@ class Controls(ControlsExt):
     self.desired_curvature = 0.0
     self.roll = 0.0
 
+    self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController")
     self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl")
     self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative")
     self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer")
@@ -89,6 +90,7 @@ class Controls(ControlsExt):
     self.param_counter += 1
     if self.param_counter >= 100:
       self.param_counter = 0
+      self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController")
       self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer")
       self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl")
       self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative")
@@ -160,7 +162,7 @@ class Controls(ControlsExt):
     steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                   self.steer_limited_by_controls, self.desired_curvature,
                                                                   self.calibrated_pose, curvature_limited)  # TODO what if not available
-    actuators.curvature = float(curvature)
+    actuators.curvature = float(curvature) if self.enable_curvature_controller else self.desired_curvature
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)
     # Ensure no NaNs/Infs
@@ -218,7 +220,7 @@ class Controls(ControlsExt):
         self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
                                               STEER_ANGLE_SATURATION_THRESHOLD
       elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
-        self.steer_limited_by_controls = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) > 1e-5
+        self.steer_limited_by_controls = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) > CURVATURE_SATURATION_THRESHOLD
       else:
         self.steer_limited_by_controls = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
 

selfdrive/controls/lib/latcontrol_curvature.py

@@ -5,6 +5,8 @@ from cereal import log
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.common.pid import PIDController
 
+CURVATURE_SATURATION_THRESHOLD = 0.4e-5 # rad/m
+
 
 class LatControlCurvature(LatControl):
   def __init__(self, CP, CP_SP, CI):
@@ -39,6 +41,6 @@ class LatControlCurvature(LatControl):
       pid_log.output = float(output_curvature)
       pid_log.actualCurvature = float(actual_curvature)
       pid_log.desiredCurvature = float(desired_curvature)
-      pid_log.saturated = bool(self._check_saturation(self.curvature_max - abs(output_curvature) < 1e-5, CS, steer_limited_by_controls, curvature_limited))
+      pid_log.saturated = bool(self._check_saturation(self.curvature_max - abs(output_curvature) < CURVATURE_SATURATION_THRESHOLD, CS, steer_limited_by_controls, curvature_limited))
 
     return 0.0, 0.0, output_curvature, pid_log

selfdrive/ui/qt/offroad/settings.cc

@@ -47,6 +47,13 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       "../assets/icons/disengage_on_accelerator.svg",
       false,
     },
+    {
+      "EnableCurvatureController",
+      tr("Enable Curvature Controller"),
+      tr("Enables curvature post-processing<br>"),
+      "../assets/icons/chffr_wheel.png",
+      false,
+    },
     {
       "EnableSpeedLimitControl",
       tr("Enable Speed Limit Control"),