diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6200c7718..5ea008744 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -623,19 +623,31 @@ class Controls: # Update Torque Params if self.CP.lateralTuning.which() == 'torque': torque_params = self.sm['liveTorqueParameters'] - if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune): - self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, - torque_params.frictionCoefficientFiltered) - elif self.frogpilot_toggles.use_custom_friction or self.frogpilot_toggles.use_custom_latAccelFactor: - lat_accel_factor = self.frogpilot_toggles.latAccelFactor if self.frogpilot_toggles.use_custom_latAccelFactor else self.CP.lateralTuning.torque.latAccelFactor - friction = self.frogpilot_toggles.friction if self.frogpilot_toggles.use_custom_friction else self.CP.lateralTuning.torque.friction - self.LaC.update_live_torque_params(lat_accel_factor, self.CP.lateralTuning.torque.latAccelOffset, friction) - else: - # Use manual parameter values from settings panel - lat_accel_factor = self.params.get_float("SteerLatAccel") - friction = self.params.get_float("SteerFriction") - steer_ratio = self.params.get_float("SteerRatio") - self.LaC.update_live_torque_params(lat_accel_factor, self.CP.lateralTuning.torque.latAccelOffset, friction) + + allow_lat_accel_learning = self.CP.carName in ['toyota', 'hyundai'] + allow_friction_learning = (allow_lat_accel_learning or self.CP.carName in ['gm']) + use_live_params = self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune) + + # Defaults pulled from manual tuning values + lat_accel_factor = self.params.get_float("SteerLatAccel") + friction = self.params.get_float("SteerFriction") + lat_accel_offset = self.CP.lateralTuning.torque.latAccelOffset + + # Apply user overrides first + if self.frogpilot_toggles.use_custom_latAccelFactor: + lat_accel_factor = self.frogpilot_toggles.latAccelFactor + if self.frogpilot_toggles.use_custom_friction: + friction = self.frogpilot_toggles.friction + + # Layer in live values only for parameters the platform allows to learn and only when not overridden + if use_live_params: + if allow_lat_accel_learning and not self.frogpilot_toggles.use_custom_latAccelFactor: + lat_accel_factor = torque_params.latAccelFactorFiltered + lat_accel_offset = torque_params.latAccelOffsetFiltered + if allow_friction_learning and not self.frogpilot_toggles.use_custom_friction: + friction = torque_params.frictionCoefficientFiltered + + self.LaC.update_live_torque_params(lat_accel_factor, lat_accel_offset, friction) if self.sm.updated['liveDelay'] and hasattr(self.LaC, "update_live_delay"): self.LaC.update_live_delay(self.sm['liveDelay'].lateralDelay) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 160c9f72f..435d6b578 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -34,7 +34,8 @@ MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches -ALLOWED_CARS = ['toyota', 'hyundai'] +FULL_AUTO_CARS = ['toyota', 'hyundai'] +FRICTION_ONLY_CARS = ['gm'] def slope2rot(slope): @@ -72,7 +73,10 @@ class TorqueEstimator(ParameterEstimator): self.offline_friction = 0.0 self.offline_latAccelFactor = 0.0 self.resets = 0.0 - self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque' + self.allow_lat_accel_learning = CP.carName in FULL_AUTO_CARS and CP.lateralTuning.which() == 'torque' + self.allow_friction_learning = (self.allow_lat_accel_learning or CP.carName in FRICTION_ONLY_CARS) \ + and CP.lateralTuning.which() == 'torque' + self.use_params = self.allow_friction_learning if CP.lateralTuning.which() == 'torque': self.offline_friction = CP.lateralTuning.torque.friction @@ -104,11 +108,11 @@ class TorqueEstimator(ParameterEstimator): cache_CP = msg if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION): if cache_ltp.liveValid: - initial_params = { - 'latAccelFactor': cache_ltp.latAccelFactorFiltered, - 'latAccelOffset': cache_ltp.latAccelOffsetFiltered, - 'frictionCoefficient': cache_ltp.frictionCoefficientFiltered - } + if self.allow_lat_accel_learning: + initial_params['latAccelFactor'] = cache_ltp.latAccelFactorFiltered + initial_params['latAccelOffset'] = cache_ltp.latAccelOffsetFiltered + if self.allow_friction_learning: + initial_params['frictionCoefficient'] = cache_ltp.frictionCoefficientFiltered initial_params['points'] = cache_ltp.points self.decay = cache_ltp.decay self.filtered_points.load_points(initial_params['points']) @@ -155,6 +159,10 @@ class TorqueEstimator(ParameterEstimator): def update_params(self, params): self.decay = min(self.decay + DT_MDL, MAX_FILTER_DECAY) for param, value in params.items(): + if param.startswith('latAccel') and not self.allow_lat_accel_learning: + continue + if param == 'frictionCoefficient' and not self.allow_friction_learning: + continue self.filtered_params[param].update(value) self.filtered_params[param].update_alpha(self.decay)