diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index f7780834b0..21f4b87529 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -24,14 +24,16 @@ 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 # 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) +INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored +INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one POSENET_STD_INITIAL_VALUE = 10.0 POSENET_STD_HIST_HALF = 20 +def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency): + return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency)) + + def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool): assert len(values) == len(stds) == 3 measurement.x, measurement.y, measurement.z = map(float, values) @@ -269,11 +271,11 @@ def main(): filter_initialized = 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} + input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services} + input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services} + input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services} initial_pose = params.get("LocationFilterInitialState") if initial_pose is not None: @@ -306,19 +308,20 @@ def main(): continue if res == HandleLogResult.TIMING_INVALID: - observation_timing_invalid[which] += 1 + print(f"Observation {which} ignored due to failed timing check") + observation_input_invalid[which] += 1 + print(observation_input_invalid[which]) elif res == HandleLogResult.INPUT_INVALID: + print(f"Observation {which} ignored due to failed sanity check") observation_input_invalid[which] += 1 else: 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) - 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 + critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services) + inputs_valid = sm.all_valid() and critical_service_inputs_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) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py deleted file mode 100644 index d0b1a82988..0000000000 --- a/selfdrive/locationd/test/test_locationd.py +++ /dev/null @@ -1,56 +0,0 @@ -import capnp - -import cereal.messaging as messaging -from openpilot.common.params import Params - -from openpilot.system.manager.process_config import managed_processes - - -class TestLocationdProc: - LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', - 'accelerometer', 'gyroscope', 'magnetometer'] - - def setup_method(self): - self.pm = messaging.PubMaster(self.LLD_MSGS) - - self.params = Params() - self.params.put_bool("UbloxAvailable", True) - managed_processes['locationd'].prepare() - managed_processes['locationd'].start() - - def teardown_method(self): - managed_processes['locationd'].stop() - - def get_msg(self, name, t): - try: - msg = messaging.new_message(name) - except capnp.lib.capnp.KjException: - msg = messaging.new_message(name, 0) - - if name == "gpsLocationExternal": - msg.gpsLocationExternal.flags = 1 - msg.gpsLocationExternal.hasFix = True - msg.gpsLocationExternal.verticalAccuracy = 1.0 - msg.gpsLocationExternal.speedAccuracy = 1.0 - msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 - msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] - msg.gpsLocationExternal.latitude = float(self.lat) - msg.gpsLocationExternal.longitude = float(self.lon) - msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 - msg.gpsLocationExternal.altitude = float(self.alt) - #if name == "gnssMeasurements": - # msg.gnssMeasurements.measTime = t - # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] - # msg.gnssMeasurements.positionECEF.std = [0,0,0] - # msg.gnssMeasurements.positionECEF.valid = True - # msg.gnssMeasurements.velocityECEF.value = [] - # msg.gnssMeasurements.velocityECEF.std = [0,0,0] - # msg.gnssMeasurements.velocityECEF.valid = True - elif name == 'cameraOdometry': - msg.cameraOdometry.rot = [0.0, 0.0, 0.0] - msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] - msg.cameraOdometry.trans = [0.0, 0.0, 0.0] - msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] - msg.logMonoTime = t - msg.valid = True - return msg diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py index bf5e571f27..0ea7ac183f 100644 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/selfdrive/locationd/test/test_locationd_scenarios.py @@ -23,8 +23,10 @@ class Scenario(Enum): BASE = 'base' GYRO_OFF = 'gyro_off' GYRO_SPIKE_MIDWAY = 'gyro_spike_midway' + GYRO_CONSISTENT_SPIKES = 'gyro_consistent_spikes' ACCEL_OFF = 'accel_off' ACCEL_SPIKE_MIDWAY = 'accel_spike_midway' + ACCEL_CONSISTENT_SPIKES = 'accel_consistent_spikes' SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes' SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes' @@ -63,18 +65,20 @@ def run_scenarios(scenario, logs): elif scenario == Scenario.GYRO_OFF: logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime) - elif scenario == Scenario.GYRO_SPIKE_MIDWAY: + elif scenario == Scenario.GYRO_SPIKE_MIDWAY or scenario == Scenario.GYRO_CONSISTENT_SPIKES: def gyro_spike(msg): msg.gyroscope.gyroUncalibrated.v[0] += 3.0 - logs = modify_logs_midway(logs, 'gyroscope', 1, gyro_spike) + count = 1 if scenario == Scenario.GYRO_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT + logs = modify_logs_midway(logs, 'gyroscope', count, gyro_spike) elif scenario == Scenario.ACCEL_OFF: logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime) - elif scenario == Scenario.ACCEL_SPIKE_MIDWAY: + elif scenario == Scenario.ACCEL_SPIKE_MIDWAY or scenario == Scenario.ACCEL_CONSISTENT_SPIKES: def acc_spike(msg): - msg.accelerometer.acceleration.v[0] += 10.0 - logs = modify_logs_midway(logs, 'accelerometer', 1, acc_spike) + msg.accelerometer.acceleration.v[0] += 100.0 + count = 1 if scenario == Scenario.ACCEL_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT + logs = modify_logs_midway(logs, 'accelerometer', count, acc_spike) elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES: def timing_spike(msg): @@ -121,7 +125,7 @@ class TestLocationdScenarios: assert np.allclose(replayed_data['roll'], 0.0) assert np.all(replayed_data['sensors_flag'] == 0.0) - def test_gyro_spikes(self): + def test_gyro_spike(self): """ Test: a gyroscope spike in the middle of the segment Expected Result: @@ -132,8 +136,17 @@ class TestLocationdScenarios: orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) - assert np.diff(replayed_data['inputs_flag'])[499] == -1.0 - assert np.diff(replayed_data['inputs_flag'])[704] == 1.0 + assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag']) + assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag']) + + def test_consistent_gyro_spikes(self): + """ + Test: consistent timing spikes for N gyroscope messages in the middle of the segment + Expected Result: inputsOK becomes False after N of bad measurements + """ + orig_data, replayed_data = run_scenarios(Scenario.GYRO_CONSISTENT_SPIKES, self.logs) + assert np.diff(replayed_data['inputs_flag'])[501] == -1.0 + assert np.diff(replayed_data['inputs_flag'])[708] == 1.0 def test_accel_off(self): """ @@ -148,7 +161,7 @@ class TestLocationdScenarios: assert np.allclose(replayed_data['roll'], 0.0) assert np.all(replayed_data['sensors_flag'] == 0.0) - def test_accel_spikes(self): + def test_accel_spike(self): """ ToDo: Test: an accelerometer spike in the middle of the segment @@ -173,5 +186,5 @@ class TestLocationdScenarios: Expected Result: inputsOK becomes False after N of bad measurements """ orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs) - assert np.diff(replayed_data['inputs_flag'])[500] == -1.0 - assert np.diff(replayed_data['inputs_flag'])[787] == 1.0 + assert np.diff(replayed_data['inputs_flag'])[501] == -1.0 + assert np.diff(replayed_data['inputs_flag'])[707] == 1.0