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

calibrationd: Pitch spread different tolerance (#30065)

Browse Source 
* calibrationd: Pitch spread different tolerance

* Improve calibrationd tests
old-commit-hash: 2162d149e8c8c6a4ac3b1acaf9462ff2f61aefbc
This commit is contained in:
Harald Schäfer
2023-09-28 14:27:50 -07:00
committed by GitHub
parent 2f0066a458
commit 25e5c57dfe
2 changed files with 52 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+4 -2
View File
@@ -31,7 +31,8 @@ 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
MAX_ALLOWED_SPREAD = np.radians(2)
MAX_ALLOWED_YAW_SPREAD = np.radians(2)
MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
RPY_INIT = np.array([0.0,0.0,0.0])
WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
HEIGHT_INIT = np.array([1.22])
@@ -156,7 +157,8 @@ class Calibrator:
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
# TODO: add height spread check with smooth transition too
if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated:
spread_too_high = self.calib_spread[1] > MAX_ALLOWED_PITCH_SPREAD or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
if spread_too_high and self.cal_status == log.LiveCalibrationData.Status.calibrated:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
self.cal_status = log.LiveCalibrationData.Status.recalibrating
selfdrive/locationd/test/test_calibrationd.py
+48 -70
View File
@@ -8,9 +8,29 @@ import cereal.messaging as messaging
from cereal import log
from openpilot.common.params import Params
from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \
MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT
MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD
def process_messages(c, cam_odo_calib, cycles,
cam_odo_speed=MIN_SPEED_FILTER + 1,
carstate_speed=MIN_SPEED_FILTER + 1,
cam_odo_yr=0.0,
cam_odo_speed_std=1e-3,
cam_odo_height_std=1e-3):
old_rpy_weight_prev = 0.0
for _ in range(cycles):
assert (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(carstate_speed)
c.handle_cam_odom([cam_odo_speed,
np.sin(cam_odo_calib[2]) * cam_odo_speed,
-np.sin(cam_odo_calib[1]) * cam_odo_speed],
[0.0, 0.0, cam_odo_yr],
[0.0, 0.0, 0.0],
[cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std],
[0.0, 0.0, HEIGHT_INIT.item()],
[cam_odo_height_std, cam_odo_height_std, cam_odo_height_std])
class TestCalibrationd(unittest.TestCase):
def test_read_saved_params(self):
@@ -28,14 +48,7 @@ class TestCalibrationd(unittest.TestCase):
def test_calibration_basics(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED)
self.assertEqual(c.valid_blocks, INPUTS_WANTED)
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
@@ -44,22 +57,8 @@ class TestCalibrationd(unittest.TestCase):
def test_calibration_low_speed_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER - 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1)
self.assertEqual(c.valid_blocks, 0)
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
@@ -67,14 +66,7 @@ class TestCalibrationd(unittest.TestCase):
def test_calibration_yaw_rate_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, MAX_YAW_RATE_FILTER ],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER)
self.assertEqual(c.valid_blocks, 0)
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
@@ -82,58 +74,44 @@ class TestCalibrationd(unittest.TestCase):
def test_calibration_speed_std_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e3, 1e3, 1e3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3)
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, np.zeros(3))
def test_calibration_speed_std_height_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e3, 1e3, 1e3])
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3)
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, np.zeros(3))
def test_calibration_auto_reset(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, INPUTS_WANTED)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3)
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10)
self.assertEqual(c.valid_blocks, INPUTS_NEEDED + 1)
self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.calibrated)
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
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],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10)
self.assertEqual(c.valid_blocks, 1)
self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, -0.05], atol=1e-2)
np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2)
c = Calibrator(param_put=False)
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10)
self.assertEqual(c.valid_blocks, 1)
self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2)
if __name__ == "__main__":
unittest.main()