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

locationd: timing spikes resiliance (#34080)

Browse Source 
* Locationd scenario for timing spike

* Add test for consistent timing spike

* Resiliance to bad timing

* Test update

* Refactor test

* fix comment

* Decay based on frequency

* Fix

* Update comment

* Only for critical services

* Fix tests
This commit is contained in:
Kacper Rączy
2024-11-26 05:46:05 +01:00
committed by GitHub
parent 29577a3346
commit eccdf8d880
2 changed files with 63 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.py
+18 -9
View File
@@ -8,6 +8,7 @@ from enum import Enum
from collections import defaultdict
from cereal import log, messaging
from cereal.services import SERVICE_LIST
from openpilot.common.transformations.orientation import rot_from_euler
from openpilot.common.realtime import config_realtime_process
from openpilot.common.params import Params
@@ -23,8 +24,10 @@ MIN_STD_SANITY_CHECK = 1e-5 # m or rad
MAX_FILTER_REWIND_TIME = 0.8 # s
MAX_SENSOR_TIME_DIFF = 0.1 # s
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
INPUT_INVALID_THRESHOLD = 0.5
INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after a bad input
INPUT_INVALID_THRESHOLD = 0.5 # 0 bad inputs ignored
TIMING_INVALID_THRESHOLD = 2.5 # 2 bad timings ignored
INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after exceeding allowed bad inputs by one (at 100hz)
TIMING_INVALID_DECAY = 0.9990 # ~2 secs to resume after exceeding allowed bad timings by one (at 100hz)
POSENET_STD_INITIAL_VALUE = 10.0
POSENET_STD_HIST_HALF = 20
@@ -265,10 +268,13 @@ def main():
estimator = LocationEstimator(DEBUG)
filter_initialized = False
critcal_services = ["accelerometer", "gyroscope", "liveCalibration", "cameraOdometry"]
observation_timing_invalid = False
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
observation_timing_invalid = defaultdict(int)
observation_input_invalid = defaultdict(int)
input_invalid_decay = {s: INPUT_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
timing_invalid_decay = {s: TIMING_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services}
initial_pose = params.get("LocationFilterInitialState")
if initial_pose is not None:
initial_pose = json.loads(initial_pose)
@@ -282,8 +288,6 @@ def main():
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
if filter_initialized:
observation_timing_invalid = False
msgs = []
for msg in acc_msgs + gyro_msgs:
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
@@ -298,18 +302,23 @@ def main():
if valid:
t = log_mono_time * 1e-9
res = estimator.handle_log(t, which, msg)
if which not in critcal_services:
continue
if res == HandleLogResult.TIMING_INVALID:
observation_timing_invalid = True
observation_timing_invalid[which] += 1
elif res == HandleLogResult.INPUT_INVALID:
observation_input_invalid[which] += 1
else:
observation_input_invalid[which] *= INPUT_INVALID_DECAY
observation_input_invalid[which] *= input_invalid_decay[which]
observation_timing_invalid[which] *= timing_invalid_decay[which]
else:
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
if sm.updated["cameraOdometry"]:
critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid and not observation_timing_invalid
critical_service_timing_valid = all(observation_timing_invalid[s] < TIMING_INVALID_THRESHOLD for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid and critical_service_timing_valid
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)