mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 02:52:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

torqued: Update reset logic (#27103)

Browse Source 
* update reset logic

* bugfix
old-commit-hash: 9201267fb7ba0080a3c508a6a258f76cbc265079
This commit is contained in:
Vivek Aithal
2023-01-26 14:48:46 -08:00
committed by GitHub
parent 499df99c9e
commit 905cd786f1
1 changed files with 10 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/torqued.py
+10 -22
View File
@@ -31,8 +31,6 @@ MAX_FILTER_DECAY = 250
LAT_ACC_THRESHOLD = 1
STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
MAX_RESETS = 5.0
MAX_INVALID_THRESHOLD = 10
MIN_ENGAGE_BUFFER = 2 # secs
VERSION = 1 # bump this to invalidate old parameter caches
@@ -173,7 +171,6 @@ class TorqueEstimator:
def reset(self):
self.resets += 1.0
self.invalid_values_tracker = 0.0
self.decay = MIN_FILTER_DECAY
self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total)
@@ -198,12 +195,6 @@ class TorqueEstimator:
self.filtered_params[param].update(value)
self.filtered_params[param].update_alpha(self.decay)
def is_sane(self, latAccelFactor, latAccelOffset, friction):
if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, friction]]):
return False
return (self.max_friction >= friction >= self.min_friction) and\
(self.max_lataccel_factor >= latAccelFactor >= self.min_lataccel_factor)
def handle_log(self, t, which, msg):
if which == "carControl":
self.raw_points["carControl_t"].append(t + self.lag)
@@ -233,23 +224,20 @@ class TorqueEstimator:
liveTorqueParameters.useParams = self.use_params
if self.filtered_points.is_valid():
latAccelFactor, latAccelOffset, friction_coeff = self.estimate_params()
latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params()
liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
liveTorqueParameters.frictionCoefficientRaw = float(friction_coeff)
liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
if self.is_sane(latAccelFactor, latAccelOffset, friction_coeff):
liveTorqueParameters.liveValid = True
self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': friction_coeff})
self.invalid_values_tracker = max(0.0, self.invalid_values_tracker - 0.5)
else:
cloudlog.exception("Live torque parameters are outside acceptable bounds.")
if any([val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]]):
cloudlog.exception("Live torque parameters are invalid.")
liveTorqueParameters.liveValid = False
self.invalid_values_tracker += 1.0
# Reset when ~10 invalid over 5 secs
if self.invalid_values_tracker > MAX_INVALID_THRESHOLD:
# Do not reset the filter as it may cause a drastic jump, just reset points
self.reset()
self.reset()
else:
liveTorqueParameters.liveValid = True
latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor)
frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction)
self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff})
else:
liveTorqueParameters.liveValid = False