From 122118ccd43ae42fef678e6392394f410145bbaf Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 3 Aug 2020 15:51:56 -0700 Subject: [PATCH] add fall filter and less FP on posenet (#1971) * add fall filter and less FP on posenet * add alert * list Co-authored-by: Adeeb Shihadeh old-commit-hash: 490ee5268712c38f738161cab939c02573581ebf --- cereal | 2 +- selfdrive/controls/controlsd.py | 2 ++ selfdrive/controls/lib/events.py | 5 +++++ selfdrive/locationd/locationd.py | 22 +++++++++------------- 4 files changed, 17 insertions(+), 14 deletions(-) diff --git a/cereal b/cereal index 10a25c8b98..61d7488e55 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 10a25c8b98ad295308e6825cef7cf42dff6f4396 +Subproject commit 61d7488e55fcaa3760d9ccc9d6589b30cbe4a6c1 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5443f8d382..2546a598af 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -216,6 +216,8 @@ class Controls: self.events.add(EventName.vehicleModelInvalid) if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) + if not self.sm['liveLocationKalman'].deviceStable: + self.events.add(EventName.deviceFalling) if not self.sm['frame'].recoverState < 2: # counter>=2 is active self.events.add(EventName.focusRecoverActive) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index eb943c1acc..205c6a1722 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -655,6 +655,11 @@ EVENTS = { ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"), }, + EventName.deviceFalling: { + ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"), + ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), + }, + EventName.lowMemory: { ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"), ET.PERMANENT: Alert( diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index ab5f06c0f2..369b68b887 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -69,6 +69,7 @@ class Localizer(): self.unix_timestamp_millis = 0 self.last_gps_fix = 0 + self.device_fell = False @staticmethod def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov): @@ -142,21 +143,12 @@ class Localizer(): def liveLocationMsg(self): fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P) - - #if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0): - # self.posenet_invalid_count += 1 - #else: - # self.posenet_invalid_count = 0 - #fix.posenetOK = self.posenet_invalid_count < 4 - - # experimentally found these values + # experimentally found these values, no false positives in 20k minutes of driving old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:]) - std_spike = new_mean/old_mean > 4 and new_mean > 5 + std_spike = new_mean/old_mean > 4 and new_mean > 7 - if std_spike and self.car_speed > 5: - fix.posenetOK = False - else: - fix.posenetOK = True + fix.posenetOK = not (std_spike and self.car_speed > 5) + fix.deviceStable = not self.device_fell #fix.gpsWeek = self.time.week #fix.gpsTimeOfWeek = self.time.tow @@ -251,6 +243,10 @@ class Localizer(): # Accelerometer if sensor_reading.sensor == 1 and sensor_reading.type == 1: + # check if device fell, estimate 10 for g + # 40g is a good filter for falling detection, no false positives in 20k minutes of driving + self.device_fell = abs(sensor_reading.acceleration.v[0] - 10) > 40 + self.acc_counter += 1 if self.acc_counter % SENSOR_DECIMATION == 0: v = sensor_reading.acceleration.v