mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-01 03:22:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Calibrationd: fix reset smoothing (#27992)

Browse Source 
* Add test for smoothing

* fix smooth cycles

* fix calib reset smoothing
old-commit-hash: ee15d6b1d1b2f90815d0cbf3c0f6b5adfdb0a59f
This commit is contained in:
Harald Schäfer
2023-04-21 06:53:54 +00:00
committed by GitHub
parent 298edb7704
commit 44aa799d91
2 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+2 -2
View File
@@ -25,7 +25,7 @@ MAX_VEL_ANGLE_STD = np.radians(0.25)
MAX_YAW_RATE_FILTER = np.radians(2) # per second
# This is at model frequency, blocks needed for efficiency
SMOOTH_CYCLES = 400
SMOOTH_CYCLES = 10
BLOCK_SIZE = 100
INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration
INPUTS_WANTED = 50 # We want a little bit more than we need for stability
@@ -162,7 +162,7 @@ class Calibrator:
rot: List[float],
wide_from_device_euler: List[float],
trans_std: List[float]) -> Optional[np.ndarray]:
self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
angle_std_threshold = MAX_VEL_ANGLE_STD
selfdrive/locationd/test/test_calibrationd.py
+5 -2
View File
@@ -6,7 +6,7 @@ import numpy as np
import cereal.messaging as messaging
from common.params import Params
from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER
from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES
class TestCalibrationd(unittest.TestCase):
@@ -86,7 +86,10 @@ class TestCalibrationd(unittest.TestCase):
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, INPUTS_WANTED)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
for _ in range(BLOCK_SIZE):
old_rpy_weight_prev = 0.0
for _ in range(BLOCK_SIZE + 10):
self.assertLess(old_rpy_weight_prev - c.old_rpy_weight, 1/SMOOTH_CYCLES + 1e-3)
old_rpy_weight_prev = c.old_rpy_weight
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0],
[0.0, 0.0, 0.0],