sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
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
pull/27994/head
Harald Schäfer 3 years ago committed by GitHub
parent f7e02c28d7
commit ee15d6b1d1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 7 additions and 4 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      selfdrive/locationd/calibrationd.py
  2. 7
      selfdrive/locationd/test/test_calibrationd.py

4
selfdrive/locationd/calibrationd.py
Unescape View File

@ -25,7 +25,7 @@ MAX_VEL_ANGLE_STD = np.radians(0.25)
MAX_YAW_RATE_FILTER = np.radians(2) # per second MAX_YAW_RATE_FILTER = np.radians(2) # per second
# This is at model frequency, blocks needed for efficiency # This is at model frequency, blocks needed for efficiency
SMOOTH_CYCLES = 400 SMOOTH_CYCLES = 10
BLOCK_SIZE = 100 BLOCK_SIZE = 100
INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration
INPUTS_WANTED = 50 # We want a little bit more than we need for stability INPUTS_WANTED = 50 # We want a little bit more than we need for stability
@ -162,7 +162,7 @@ class Calibrator:
rot: List[float], rot: List[float],
wide_from_device_euler: List[float], wide_from_device_euler: List[float],
trans_std: List[float]) -> Optional[np.ndarray]: 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)) 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 angle_std_threshold = MAX_VEL_ANGLE_STD

7
selfdrive/locationd/test/test_calibrationd.py
Unescape View File

@ -6,7 +6,7 @@ import numpy as np
import cereal.messaging as messaging import cereal.messaging as messaging
from common.params import Params 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): class TestCalibrationd(unittest.TestCase):
@ -86,7 +86,10 @@ class TestCalibrationd(unittest.TestCase):
[1e-3, 1e-3, 1e-3]) [1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, INPUTS_WANTED) self.assertEqual(c.valid_blocks, INPUTS_WANTED)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) 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_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0], c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0],
[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],

Write Preview
Loading…
Cancel
Save