From b79c2bff546da9b0ec9228882be567d01cfaaece Mon Sep 17 00:00:00 2001 From: Edison-CBS Date: Sun, 8 Sep 2024 22:13:30 +0800 Subject: [PATCH] rm redundant files --- selfdrive/car/tests/routes.py | 295 ------ selfdrive/controls/lib/alertmanager.py | 61 -- selfdrive/controls/lib/alerts_offroad.json | 52 - selfdrive/controls/lib/events.py | 995 ------------------ .../controls/lib/tests/test_alertmanager.py | 39 - selfdrive/controls/tests/test_alerts.py | 130 --- selfdrive/controls/tests/test_cruise_speed.py | 151 --- selfdrive/controls/tests/test_startup.py | 121 --- .../controls/tests/test_state_machine.py | 102 -- selfdrive/locationd/locationd.cc | 716 ------------- selfdrive/locationd/locationd.h | 99 -- selfdrive/locationd/models/live_kf.cc | 122 --- selfdrive/locationd/models/live_kf.h | 66 -- selfdrive/locationd/models/live_kf.py | 242 ----- 14 files changed, 3191 deletions(-) delete mode 100755 selfdrive/car/tests/routes.py delete mode 100644 selfdrive/controls/lib/alertmanager.py delete mode 100644 selfdrive/controls/lib/alerts_offroad.json delete mode 100755 selfdrive/controls/lib/events.py delete mode 100644 selfdrive/controls/lib/tests/test_alertmanager.py delete mode 100644 selfdrive/controls/tests/test_alerts.py delete mode 100644 selfdrive/controls/tests/test_cruise_speed.py delete mode 100644 selfdrive/controls/tests/test_startup.py delete mode 100644 selfdrive/controls/tests/test_state_machine.py delete mode 100644 selfdrive/locationd/locationd.cc delete mode 100644 selfdrive/locationd/locationd.h delete mode 100644 selfdrive/locationd/models/live_kf.cc delete mode 100644 selfdrive/locationd/models/live_kf.h delete mode 100755 selfdrive/locationd/models/live_kf.py diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py deleted file mode 100755 index c9f57a4c712b7f..00000000000000 --- a/selfdrive/car/tests/routes.py +++ /dev/null @@ -1,295 +0,0 @@ -#!/usr/bin/env python3 -from typing import NamedTuple - -from opendbc.car.chrysler.values import CAR as CHRYSLER -from opendbc.car.gm.values import CAR as GM -from opendbc.car.ford.values import CAR as FORD -from opendbc.car.honda.values import CAR as HONDA -from opendbc.car.hyundai.values import CAR as HYUNDAI -from opendbc.car.nissan.values import CAR as NISSAN -from opendbc.car.mazda.values import CAR as MAZDA -from opendbc.car.subaru.values import CAR as SUBARU -from opendbc.car.toyota.values import CAR as TOYOTA -from opendbc.car.values import Platform -from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN -from opendbc.car.body.values import CAR as COMMA - -# TODO: add routes for these cars -non_tested_cars = [ - FORD.FORD_F_150_MK14, - GM.CADILLAC_ATS, - GM.HOLDEN_ASTRA, - GM.CHEVROLET_MALIBU, - HYUNDAI.GENESIS_G90, - HONDA.HONDA_ODYSSEY_CHN, - VOLKSWAGEN.VOLKSWAGEN_CRAFTER_MK2, # need a route from an ACC-equipped Crafter - SUBARU.SUBARU_FORESTER_HYBRID, -] - - -class CarTestRoute(NamedTuple): - route: str - car_model: Platform | None - segment: int | None = None - - -routes = [ - CarTestRoute("efdf9af95e71cd84|2022-05-13--19-03-31", COMMA.COMMA_BODY), - - CarTestRoute("0c94aa1e1296d7c6|2021-05-05--19-48-37", CHRYSLER.JEEP_GRAND_CHEROKEE), - CarTestRoute("91dfedae61d7bd75|2021-05-22--20-07-52", CHRYSLER.JEEP_GRAND_CHEROKEE_2019), - CarTestRoute("420a8e183f1aed48|2020-03-05--07-15-29", CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID), # 2017 - CarTestRoute("43a685a66291579b|2021-05-27--19-47-29", CHRYSLER.CHRYSLER_PACIFICA_2018), - CarTestRoute("378472f830ee7395|2021-05-28--07-38-43", CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID), - CarTestRoute("8190c7275a24557b|2020-01-29--08-33-58", CHRYSLER.CHRYSLER_PACIFICA_2019_HYBRID), - CarTestRoute("3d84727705fecd04|2021-05-25--08-38-56", CHRYSLER.CHRYSLER_PACIFICA_2020), - CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500_5TH_GEN), - CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD_5TH_GEN, segment=6), - CarTestRoute("3379c85aeedc8285|2023-12-07--17-49-39", CHRYSLER.DODGE_DURANGO), - - CarTestRoute("54827bf84c38b14f|2023-01-25--14-14-11", FORD.FORD_BRONCO_SPORT_MK1), - CarTestRoute("f8eaaccd2a90aef8|2023-05-04--15-10-09", FORD.FORD_ESCAPE_MK4), - CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.FORD_EXPLORER_MK6), - CarTestRoute("e886087f430e7fe7|2023-06-16--23-06-36", FORD.FORD_FOCUS_MK4), - CarTestRoute("bd37e43731e5964b|2023-04-30--10-42-26", FORD.FORD_MAVERICK_MK1), - CarTestRoute("112e4d6e0cad05e1|2023-11-14--08-21-43", FORD.FORD_F_150_LIGHTNING_MK1), - CarTestRoute("83a4e056c7072678|2023-11-13--16-51-33", FORD.FORD_MUSTANG_MACH_E_MK1), - CarTestRoute("37998aa0fade36ab/00000000--48f927c4f5", FORD.FORD_RANGER_MK2), - #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), - - CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.GMC_ACADIA), - CarTestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL), - CarTestRoute("75a6bcb9b8b40373|2023-03-11--22-47-33", GM.BUICK_LACROSSE), - CarTestRoute("e746f59bc96fd789|2024-01-31--22-25-58", GM.CHEVROLET_EQUINOX), - CarTestRoute("ef8f2185104d862e|2023-02-09--18-37-13", GM.CADILLAC_ESCALADE), - CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.CADILLAC_ESCALADE_ESV), - CarTestRoute("168f8b3be57f66ae|2023-09-12--21-44-42", GM.CADILLAC_ESCALADE_ESV_2019), - CarTestRoute("c950e28c26b5b168|2018-05-30--22-03-41", GM.CHEVROLET_VOLT), - CarTestRoute("f08912a233c1584f|2022-08-11--18-02-41", GM.CHEVROLET_BOLT_EUV, segment=1), - CarTestRoute("555d4087cf86aa91|2022-12-02--12-15-07", GM.CHEVROLET_BOLT_EUV, segment=14), # Bolt EV - CarTestRoute("38aa7da107d5d252|2022-08-15--16-01-12", GM.CHEVROLET_SILVERADO), - CarTestRoute("5085c761395d1fe6|2023-04-07--18-20-06", GM.CHEVROLET_TRAILBLAZER), - - CarTestRoute("0e7a2ba168465df5|2020-10-18--14-14-22", HONDA.ACURA_RDX_3G), - CarTestRoute("a74b011b32b51b56|2020-07-26--17-09-36", HONDA.HONDA_CIVIC), - CarTestRoute("a859a044a447c2b0|2020-03-03--18-42-45", HONDA.HONDA_CRV_EU), - CarTestRoute("68aac44ad69f838e|2021-05-18--20-40-52", HONDA.HONDA_CRV), - CarTestRoute("14fed2e5fa0aa1a5|2021-05-25--14-59-42", HONDA.HONDA_CRV_HYBRID), - CarTestRoute("52f3e9ae60c0d886|2021-05-23--15-59-43", HONDA.HONDA_FIT), - CarTestRoute("2c4292a5cd10536c|2021-08-19--21-32-15", HONDA.HONDA_FREED), - CarTestRoute("03be5f2fd5c508d1|2020-04-19--18-44-15", HONDA.HONDA_HRV), - CarTestRoute("320098ff6c5e4730|2023-04-13--17-47-46", HONDA.HONDA_HRV_3G), - CarTestRoute("147613502316e718/00000001--dd141a3140", HONDA.HONDA_HRV_3G), # Brazilian model - CarTestRoute("917b074700869333|2021-05-24--20-40-20", HONDA.ACURA_ILX), - CarTestRoute("08a3deb07573f157|2020-03-06--16-11-19", HONDA.HONDA_ACCORD), # 1.5T - CarTestRoute("1da5847ac2488106|2021-05-24--19-31-50", HONDA.HONDA_ACCORD), # 2.0T - CarTestRoute("085ac1d942c35910|2021-03-25--20-11-15", HONDA.HONDA_ACCORD), # 2021 with new style HUD msgs - CarTestRoute("07585b0da3c88459|2021-05-26--18-52-04", HONDA.HONDA_ACCORD), # hybrid - CarTestRoute("f29e2b57a55e7ad5|2021-03-24--20-52-38", HONDA.HONDA_ACCORD), # hybrid, 2021 with new style HUD msgs - CarTestRoute("1ad763dd22ef1a0e|2020-02-29--18-37-03", HONDA.HONDA_CRV_5G), - CarTestRoute("0a96f86fcfe35964|2020-02-05--07-25-51", HONDA.HONDA_ODYSSEY), - CarTestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.HONDA_CIVIC_BOSCH_DIESEL), - CarTestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.HONDA_INSIGHT), - CarTestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.HONDA_PILOT), - CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.HONDA_PILOT), # Passport - CarTestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.HONDA_CIVIC_BOSCH), - CarTestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), - CarTestRoute("54fd8451b3974762|2021-04-01--14-50-10", HONDA.HONDA_RIDGELINE), - CarTestRoute("2d5808fae0b38ac6|2021-09-01--17-14-11", HONDA.HONDA_E), - CarTestRoute("f44aa96ace22f34a|2021-12-22--06-22-31", HONDA.HONDA_CIVIC_2022), - - CarTestRoute("87d7f06ade479c2e|2023-09-11--23-30-11", HYUNDAI.HYUNDAI_AZERA_6TH_GEN), - CarTestRoute("66189dd8ec7b50e6|2023-09-20--07-02-12", HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN), - CarTestRoute("6fe86b4e410e4c37|2020-07-22--16-27-13", HYUNDAI.HYUNDAI_GENESIS), - CarTestRoute("b5d6dc830ad63071|2022-12-12--21-28-25", HYUNDAI.GENESIS_GV60_EV_1ST_GEN, segment=12), - CarTestRoute("70c5bec28ec8e345|2020-08-08--12-22-23", HYUNDAI.GENESIS_G70), - CarTestRoute("ca4de5b12321bd98|2022-10-18--21-15-59", HYUNDAI.GENESIS_GV70_1ST_GEN), - CarTestRoute("6b301bf83f10aa90|2020-11-22--16-45-07", HYUNDAI.GENESIS_G80), - CarTestRoute("0bbe367c98fa1538|2023-09-16--00-16-49", HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN), - CarTestRoute("f0709d2bc6ca451f|2022-10-15--08-13-54", HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN), - CarTestRoute("4dbd55df87507948|2022-03-01--09-45-38", HYUNDAI.HYUNDAI_SANTA_FE), - CarTestRoute("bf43d9df2b660eb0|2021-09-23--14-16-37", HYUNDAI.HYUNDAI_SANTA_FE_2022), - CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.HYUNDAI_SANTA_FE_HEV_2022), - CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022, segment=1), - CarTestRoute("de59124955b921d8|2023-06-24--00-12-50", HYUNDAI.KIA_CARNIVAL_4TH_GEN), - CarTestRoute("409c9409979a8abc|2023-07-11--09-06-44", HYUNDAI.KIA_CARNIVAL_4TH_GEN), # Chinese model - CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED), - CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4), - CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_G4_FL, segment=0), - CarTestRoute("f9716670b2481438|2023-08-23--14-49-50", HYUNDAI.KIA_OPTIMA_H), - CarTestRoute("6a42c1197b2a8179|2023-09-21--10-23-44", HYUNDAI.KIA_OPTIMA_H_G4_FL), - CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS), - CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.HYUNDAI_SONATA), - CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.HYUNDAI_SONATA_LF), - CarTestRoute("c344fd2492c7a9d2|2023-12-11--09-03-23", HYUNDAI.HYUNDAI_STARIA_4TH_GEN), - CarTestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.HYUNDAI_TUCSON), - CarTestRoute("db68bbe12250812c|2022-12-05--00-54-12", HYUNDAI.HYUNDAI_TUCSON_4TH_GEN), # 2023 - CarTestRoute("36e10531feea61a4|2022-07-25--13-37-42", HYUNDAI.HYUNDAI_TUCSON_4TH_GEN), # hybrid - CarTestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO), - CarTestRoute("1d0d000db3370fd0|2023-01-04--22-28-42", HYUNDAI.KIA_SORENTO_4TH_GEN, segment=5), - CarTestRoute("fc19648042eb6896|2023-08-16--11-43-27", HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, segment=14), - CarTestRoute("628935d7d3e5f4f7|2022-11-30--01-12-46", HYUNDAI.KIA_SORENTO_HEV_4TH_GEN), # plug-in hybrid - CarTestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.HYUNDAI_PALISADE), - CarTestRoute("05a8f0197fdac372|2022-10-19--14-14-09", HYUNDAI.HYUNDAI_IONIQ_5), # HDA2 - CarTestRoute("eb4eae1476647463|2023-08-26--18-07-04", HYUNDAI.HYUNDAI_IONIQ_6, segment=6), # HDA2 - CarTestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.HYUNDAI_IONIQ_PHEV_2019), - CarTestRoute("fa8db5869167f821|2021-06-10--22-50-10", HYUNDAI.HYUNDAI_IONIQ_PHEV), - CarTestRoute("e1107f9d04dfb1e2|2023-09-05--22-32-12", HYUNDAI.HYUNDAI_IONIQ_PHEV), # openpilot longitudinal enabled - CarTestRoute("2c5cf2dd6102e5da|2020-12-17--16-06-44", HYUNDAI.HYUNDAI_IONIQ_EV_2020), - CarTestRoute("610ebb9faaad6b43|2020-06-13--15-28-36", HYUNDAI.HYUNDAI_IONIQ_EV_LTD), - CarTestRoute("2c5cf2dd6102e5da|2020-06-26--16-00-08", HYUNDAI.HYUNDAI_IONIQ), - CarTestRoute("012c95f06918eca4|2023-01-15--11-19-36", HYUNDAI.HYUNDAI_IONIQ), # openpilot longitudinal enabled - CarTestRoute("ab59fe909f626921|2021-10-18--18-34-28", HYUNDAI.HYUNDAI_IONIQ_HEV_2022), - CarTestRoute("22d955b2cd499c22|2020-08-10--19-58-21", HYUNDAI.HYUNDAI_KONA), - CarTestRoute("efc48acf44b1e64d|2021-05-28--21-05-04", HYUNDAI.HYUNDAI_KONA_EV), - CarTestRoute("f90d3cd06caeb6fa|2023-09-06--17-15-47", HYUNDAI.HYUNDAI_KONA_EV), # openpilot longitudinal enabled - CarTestRoute("ff973b941a69366f|2022-07-28--22-01-19", HYUNDAI.HYUNDAI_KONA_EV_2022, segment=11), - CarTestRoute("1618132d68afc876|2023-08-27--09-32-14", HYUNDAI.HYUNDAI_KONA_EV_2ND_GEN, segment=13), - CarTestRoute("49f3c13141b6bc87|2021-07-28--08-05-13", HYUNDAI.HYUNDAI_KONA_HEV), - CarTestRoute("5dddcbca6eb66c62|2020-07-26--13-24-19", HYUNDAI.KIA_STINGER), - CarTestRoute("5b50b883a4259afb|2022-11-09--15-00-42", HYUNDAI.KIA_STINGER_2022), - CarTestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.HYUNDAI_VELOSTER), - CarTestRoute("d545129f3ca90f28|2022-10-19--09-22-54", HYUNDAI.KIA_EV6), # HDA2 - CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6), # HDA1 - CarTestRoute("9b25e8c1484a1b67|2023-04-13--10-41-45", HYUNDAI.KIA_EV6), - CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021), - CarTestRoute("c58dfc9fc16590e0|2023-01-14--13-51-48", HYUNDAI.KIA_K5_HEV_2020), - CarTestRoute("78ad5150de133637|2023-09-13--16-15-57", HYUNDAI.KIA_K8_HEV_1ST_GEN), - CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV), - CarTestRoute("b153671049a867b3|2023-04-05--10-00-30", HYUNDAI.KIA_NIRO_EV_2ND_GEN), - CarTestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_PHEV), - CarTestRoute("23349923ba5c4e3b|2023-12-02--08-51-54", HYUNDAI.KIA_NIRO_PHEV_2022), - CarTestRoute("34a875f29f69841a|2021-07-29--13-02-09", HYUNDAI.KIA_NIRO_HEV_2021), - CarTestRoute("db04d2c63990e3ba|2023-02-08--16-52-39", HYUNDAI.KIA_NIRO_HEV_2ND_GEN), - CarTestRoute("50a2212c41f65c7b|2021-05-24--16-22-06", HYUNDAI.KIA_FORTE), - CarTestRoute("192283cdbb7a58c2|2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN), - CarTestRoute("09559f1fcaed4704|2023-11-16--02-24-57", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # openpilot longitudinal - CarTestRoute("b3537035ffe6a7d6|2022-10-17--15-23-49", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # hybrid - CarTestRoute("c5ac319aa9583f83|2021-06-01--18-18-31", HYUNDAI.HYUNDAI_ELANTRA), - CarTestRoute("734ef96182ddf940|2022-10-02--16-41-44", HYUNDAI.HYUNDAI_ELANTRA_GT_I30), - CarTestRoute("82e9cdd3f43bf83e|2021-05-15--02-42-51", HYUNDAI.HYUNDAI_ELANTRA_2021), - CarTestRoute("715ac05b594e9c59|2021-06-20--16-21-07", HYUNDAI.HYUNDAI_ELANTRA_HEV_2021), - CarTestRoute("7120aa90bbc3add7|2021-08-02--07-12-31", HYUNDAI.HYUNDAI_SONATA_HYBRID), - CarTestRoute("715ac05b594e9c59|2021-10-27--23-24-56", HYUNDAI.GENESIS_G70_2020), - CarTestRoute("6b0d44d22df18134|2023-05-06--10-36-55", HYUNDAI.GENESIS_GV80), - - CarTestRoute("00c829b1b7613dea|2021-06-24--09-10-10", TOYOTA.TOYOTA_ALPHARD_TSS2), - CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.TOYOTA_ALPHARD_TSS2), # hybrid - CarTestRoute("000cf3730200c71c|2021-05-24--10-42-05", TOYOTA.TOYOTA_AVALON), - CarTestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.TOYOTA_AVALON_2019), - CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.TOYOTA_AVALON_2019), # hybrid - CarTestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.TOYOTA_AVALON_TSS2), - CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.TOYOTA_AVALON_TSS2), # hybrid - CarTestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.TOYOTA_CAMRY), - CarTestRoute("2f37c007683e85ba|2023-09-02--14-39-44", TOYOTA.TOYOTA_CAMRY), # openpilot longitudinal, with radar CAN filter - CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.TOYOTA_CAMRY), # hybrid - CarTestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.TOYOTA_CAMRY_TSS2), - CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.TOYOTA_CAMRY_TSS2), # hybrid - CarTestRoute("4e45c89c38e8ec4d|2021-05-02--02-49-28", TOYOTA.TOYOTA_COROLLA), - CarTestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.TOYOTA_COROLLA_TSS2), - CarTestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.TOYOTA_COROLLA_TSS2), # hybrid - CarTestRoute("d2525c22173da58b|2021-04-25--16-47-04", TOYOTA.TOYOTA_PRIUS), - CarTestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.TOYOTA_RAV4), - CarTestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.TOYOTA_RAV4H), - CarTestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.TOYOTA_RAV4_TSS2), - CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.TOYOTA_RAV4_TSS2_2022), - CarTestRoute("ad5a3fa719bc2f83|2023-10-17--19-48-42", TOYOTA.TOYOTA_RAV4_TSS2_2023), - CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.TOYOTA_RAV4_TSS2), # hybrid - CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.TOYOTA_RAV4_TSS2_2022), # hybrid - CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES), - CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ES), # hybrid - CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), - CarTestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ES_TSS2), # hybrid - CarTestRoute("da23c367491f53e2|2021-05-21--09-09-11", TOYOTA.LEXUS_CTH, segment=3), - CarTestRoute("32696cea52831b02|2021-11-19--18-13-30", TOYOTA.LEXUS_RC), - CarTestRoute("ab9b64a5e5960cba|2023-10-24--17-32-08", TOYOTA.LEXUS_GS_F), - CarTestRoute("886fcd8408d570e9|2020-01-29--02-18-55", TOYOTA.LEXUS_RX), - CarTestRoute("d27ad752e9b08d4f|2021-05-26--19-39-51", TOYOTA.LEXUS_RX), # hybrid - CarTestRoute("01b22eb2ed121565|2020-02-02--11-25-51", TOYOTA.LEXUS_RX_TSS2), - CarTestRoute("b74758c690a49668|2020-05-20--15-58-57", TOYOTA.LEXUS_RX_TSS2), # hybrid - CarTestRoute("964c09eb11ca8089|2020-11-03--22-04-00", TOYOTA.LEXUS_NX), - CarTestRoute("ec429c0f37564e3c|2020-02-01--17-28-12", TOYOTA.LEXUS_NX), # hybrid - CarTestRoute("3fd5305f8b6ca765|2021-04-28--19-26-49", TOYOTA.LEXUS_NX_TSS2), - CarTestRoute("09ae96064ed85a14|2022-06-09--12-22-31", TOYOTA.LEXUS_NX_TSS2), # hybrid - CarTestRoute("4765fbbf59e3cd88|2024-02-06--17-45-32", TOYOTA.LEXUS_LC_TSS2), - CarTestRoute("0a302ffddbb3e3d3|2020-02-08--16-19-08", TOYOTA.TOYOTA_HIGHLANDER_TSS2), - CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.TOYOTA_HIGHLANDER_TSS2), # hybrid - CarTestRoute("3183cd9b021e89ce|2021-05-25--10-34-44", TOYOTA.TOYOTA_HIGHLANDER), - CarTestRoute("80d16a262e33d57f|2021-05-23--20-01-43", TOYOTA.TOYOTA_HIGHLANDER), # hybrid - CarTestRoute("eb6acd681135480d|2019-06-20--20-00-00", TOYOTA.TOYOTA_SIENNA), - CarTestRoute("2e07163a1ba9a780|2019-08-25--13-15-13", TOYOTA.LEXUS_IS), - CarTestRoute("649bf2997ada6e3a|2023-08-08--18-04-22", TOYOTA.LEXUS_IS_TSS2), - CarTestRoute("0a0de17a1e6a2d15|2020-09-21--21-24-41", TOYOTA.TOYOTA_PRIUS_TSS2), - CarTestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.TOYOTA_MIRAI), - CarTestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.TOYOTA_CHR), - CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.TOYOTA_CHR), # hybrid - CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.TOYOTA_CHR_TSS2), - CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.TOYOTA_CHR_TSS2), # openpilot longitudinal, with smartDSU - CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid - CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid, openpilot longitudinal, radar disabled - CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.TOYOTA_PRIUS_V), - - CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1), - CarTestRoute("2c68dda277d887ac|2021-05-11--15-22-20", VOLKSWAGEN.VOLKSWAGEN_ATLAS_MK1), - CarTestRoute("ffcd23abbbd02219|2024-02-28--14-59-38", VOLKSWAGEN.VOLKSWAGEN_CADDY_MK3), - CarTestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # Stock ACC - CarTestRoute("3cfdec54aa035f3f|2022-10-13--14-58-58", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # openpilot longitudinal - CarTestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.VOLKSWAGEN_JETTA_MK7), - CarTestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.VOLKSWAGEN_PASSAT_MK8), - CarTestRoute("3cfdec54aa035f3f|2022-07-19--23-45-10", VOLKSWAGEN.VOLKSWAGEN_PASSAT_NMS), - CarTestRoute("0cd0b7f7e31a3853|2021-11-03--19-30-22", VOLKSWAGEN.VOLKSWAGEN_POLO_MK6), - CarTestRoute("064d1816e448f8eb|2022-09-29--15-32-34", VOLKSWAGEN.VOLKSWAGEN_SHARAN_MK2), - CarTestRoute("7d82b2f3a9115f1f|2021-10-21--15-39-42", VOLKSWAGEN.VOLKSWAGEN_TAOS_MK1), - CarTestRoute("2744c89a8dda9a51|2021-07-24--21-28-06", VOLKSWAGEN.VOLKSWAGEN_TCROSS_MK1), - CarTestRoute("2cef8a0b898f331a|2021-03-25--20-13-57", VOLKSWAGEN.VOLKSWAGEN_TIGUAN_MK2), - CarTestRoute("a589dcc642fdb10a|2021-06-14--20-54-26", VOLKSWAGEN.VOLKSWAGEN_TOURAN_MK2), - CarTestRoute("a459f4556782eba1|2021-09-19--09-48-00", VOLKSWAGEN.VOLKSWAGEN_TRANSPORTER_T61), - CarTestRoute("0cd0b7f7e31a3853|2021-11-18--00-38-32", VOLKSWAGEN.VOLKSWAGEN_TROC_MK1), - CarTestRoute("07667b885add75fd|2021-01-23--19-48-42", VOLKSWAGEN.AUDI_A3_MK3), - CarTestRoute("6c6b466346192818|2021-06-06--14-17-47", VOLKSWAGEN.AUDI_Q2_MK1), - CarTestRoute("0cd0b7f7e31a3853|2021-12-03--03-12-05", VOLKSWAGEN.AUDI_Q3_MK2), - CarTestRoute("8f205bdd11bcbb65|2021-03-26--01-00-17", VOLKSWAGEN.SEAT_ATECA_MK1), - CarTestRoute("fc6b6c9a3471c846|2021-05-27--13-39-56", VOLKSWAGEN.SEAT_ATECA_MK1), # Leon - CarTestRoute("0bbe367c98fa1538|2023-03-04--17-46-11", VOLKSWAGEN.SKODA_FABIA_MK4), - CarTestRoute("12d6ae3057c04b0d|2021-09-15--00-04-07", VOLKSWAGEN.SKODA_KAMIQ_MK1), - CarTestRoute("12d6ae3057c04b0d|2021-09-04--21-21-21", VOLKSWAGEN.SKODA_KAROQ_MK1), - CarTestRoute("90434ff5d7c8d603|2021-03-15--12-07-31", VOLKSWAGEN.SKODA_KODIAQ_MK1), - CarTestRoute("66e5edc3a16459c5|2021-05-25--19-00-29", VOLKSWAGEN.SKODA_OCTAVIA_MK3), - CarTestRoute("026b6d18fba6417f|2021-03-26--09-17-04", VOLKSWAGEN.SKODA_KAMIQ_MK1), # Scala - CarTestRoute("b2e9858e29db492b|2021-03-26--16-58-42", VOLKSWAGEN.SKODA_SUPERB_MK3), - - CarTestRoute("3c8f0c502e119c1c|2020-06-30--12-58-02", SUBARU.SUBARU_ASCENT), - CarTestRoute("c321c6b697c5a5ff|2020-06-23--11-04-33", SUBARU.SUBARU_FORESTER), - CarTestRoute("791340bc01ed993d|2019-03-10--16-28-08", SUBARU.SUBARU_IMPREZA), - CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.SUBARU_IMPREZA_2020), - CarTestRoute("8de015561e1ea4a0|2023-08-29--17-08-31", SUBARU.SUBARU_IMPREZA), # openpilot longitudinal - # CarTestRoute("c3d1ccb52f5f9d65|2023-07-22--01-23-20", SUBARU.OUTBACK, segment=9), # gen2 longitudinal, eyesight disabled - CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.SUBARU_OUTBACK, segment=10), - CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.SUBARU_LEGACY, segment=3), - CarTestRoute("f4e3a0c511a076f4|2022-08-04--16-16-48", SUBARU.SUBARU_CROSSTREK_HYBRID, segment=2), - CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.SUBARU_FORESTER_2022, segment=4), - CarTestRoute("f3b34c0d2632aa83|2023-07-23--20-43-25", SUBARU.SUBARU_OUTBACK_2023, segment=7), - CarTestRoute("99437cef6d5ff2ee|2023-03-13--21-21-38", SUBARU.SUBARU_ASCENT_2023, segment=7), - # Pre-global, dashcam - CarTestRoute("95441c38ae8c130e|2020-06-08--12-10-17", SUBARU.SUBARU_FORESTER_PREGLOBAL), - CarTestRoute("df5ca7660000fba8|2020-06-16--17-37-19", SUBARU.SUBARU_LEGACY_PREGLOBAL), - CarTestRoute("5ab784f361e19b78|2020-06-08--16-30-41", SUBARU.SUBARU_OUTBACK_PREGLOBAL), - CarTestRoute("e19eb5d5353b1ac1|2020-08-09--14-37-56", SUBARU.SUBARU_OUTBACK_PREGLOBAL_2018), - - CarTestRoute("fbbfa6af821552b9|2020-03-03--08-09-43", NISSAN.NISSAN_XTRAIL), - CarTestRoute("5b7c365c50084530|2020-03-25--22-10-13", NISSAN.NISSAN_LEAF), - CarTestRoute("22c3dcce2dd627eb|2020-12-30--16-38-48", NISSAN.NISSAN_LEAF_IC), - CarTestRoute("059ab9162e23198e|2020-05-30--09-41-01", NISSAN.NISSAN_ROGUE), - CarTestRoute("b72d3ec617c0a90f|2020-12-11--15-38-17", NISSAN.NISSAN_ALTIMA), - - CarTestRoute("32a319f057902bb3|2020-04-27--15-18-58", MAZDA.MAZDA_CX5), - CarTestRoute("10b5a4b380434151|2020-08-26--17-11-45", MAZDA.MAZDA_CX9), - CarTestRoute("74f1038827005090|2020-08-26--20-05-50", MAZDA.MAZDA_3), - CarTestRoute("fb53c640f499b73d|2021-06-01--04-17-56", MAZDA.MAZDA_6), - CarTestRoute("f6d5b1a9d7a1c92e|2021-07-08--06-56-59", MAZDA.MAZDA_CX9_2021), - CarTestRoute("a4af1602d8e668ac|2022-02-03--12-17-07", MAZDA.MAZDA_CX5_2022), - - # Segments that test specific issues - # Controls mismatch due to standstill threshold - CarTestRoute("bec2dcfde6a64235|2022-04-08--14-21-32", HONDA.HONDA_CRV_HYBRID, segment=22), -] diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py deleted file mode 100644 index f67e269fa97854..00000000000000 --- a/selfdrive/controls/lib/alertmanager.py +++ /dev/null @@ -1,61 +0,0 @@ -import copy -import os -import json -from collections import defaultdict -from dataclasses import dataclass - -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.events import Alert - - -with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: - OFFROAD_ALERTS = json.load(f) - - -def set_offroad_alert(alert: str, show_alert: bool, extra_text: str = None) -> None: - if show_alert: - a = copy.copy(OFFROAD_ALERTS[alert]) - a['extra'] = extra_text or '' - Params().put(alert, json.dumps(a)) - else: - Params().remove(alert) - - -@dataclass -class AlertEntry: - alert: Alert | None = None - start_frame: int = -1 - end_frame: int = -1 - - def active(self, frame: int) -> bool: - return frame <= self.end_frame - -class AlertManager: - def __init__(self): - self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry) - - def add_many(self, frame: int, alerts: list[Alert]) -> None: - for alert in alerts: - entry = self.alerts[alert.alert_type] - entry.alert = alert - if not entry.active(frame): - entry.start_frame = frame - min_end_frame = entry.start_frame + alert.duration - entry.end_frame = max(frame + 1, min_end_frame) - - def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None: - current_alert = AlertEntry() - for v in self.alerts.values(): - if not v.alert: - continue - - if v.alert.event_type in clear_event_types: - v.end_frame = -1 - - # sort by priority first and then by start_frame - greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame) - if v.active(frame) and greater: - current_alert = v - - return current_alert.alert diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json deleted file mode 100644 index 6fb6c569b023f2..00000000000000 --- a/selfdrive/controls/lib/alerts_offroad.json +++ /dev/null @@ -1,52 +0,0 @@ -{ - "Offroad_TemperatureTooHigh": { - "text": "Device temperature too high. System cooling down before starting. Current internal component temperature: %1", - "severity": 0 - }, - "Offroad_ConnectivityNeededPrompt": { - "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in %1", - "severity": 0, - "_comment": "Set extra field to number of days" - }, - "Offroad_ConnectivityNeeded": { - "text": "Connect to internet to check for updates. openpilot won't automatically start until it connects to internet to check for updates.", - "severity": 1 - }, - "Offroad_UpdateFailed": { - "text": "Unable to download updates\n%1", - "severity": 1, - "_comment": "Set extra field to the failed reason." - }, - "Offroad_IsTakingSnapshot": { - "text": "Taking camera snapshots. System won't start until finished.", - "severity": 0 - }, - "Offroad_NeosUpdate": { - "text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.", - "severity": 0 - }, - "Offroad_UnofficialHardware": { - "text": "Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support.", - "severity": 1 - }, - "Offroad_StorageMissing": { - "text": "NVMe drive not mounted.", - "severity": 1 - }, - "Offroad_BadNvme": { - "text": "Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe.", - "severity": 1 - }, - "Offroad_CarUnrecognized": { - "text": "openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.", - "severity": 0 - }, - "Offroad_NoFirmware": { - "text": "openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.", - "severity": 0 - }, - "Offroad_Recalibration": { - "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.", - "severity": 0 - } -} diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py deleted file mode 100755 index d27f02794b351b..00000000000000 --- a/selfdrive/controls/lib/events.py +++ /dev/null @@ -1,995 +0,0 @@ -#!/usr/bin/env python3 -import bisect -import math -import os -from enum import IntEnum -from collections.abc import Callable - -from cereal import log, car -import cereal.messaging as messaging -from openpilot.common.conversions import Conversions as CV -from openpilot.common.git import get_short_branch -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER - -AlertSize = log.SelfdriveState.AlertSize -AlertStatus = log.SelfdriveState.AlertStatus -VisualAlert = car.CarControl.HUDControl.VisualAlert -AudibleAlert = car.CarControl.HUDControl.AudibleAlert -EventName = car.OnroadEvent.EventName - - -# Alert priorities -class Priority(IntEnum): - LOWEST = 0 - LOWER = 1 - LOW = 2 - MID = 3 - HIGH = 4 - HIGHEST = 5 - - -# Event types -class ET: - ENABLE = 'enable' - PRE_ENABLE = 'preEnable' - OVERRIDE_LATERAL = 'overrideLateral' - OVERRIDE_LONGITUDINAL = 'overrideLongitudinal' - NO_ENTRY = 'noEntry' - WARNING = 'warning' - USER_DISABLE = 'userDisable' - SOFT_DISABLE = 'softDisable' - IMMEDIATE_DISABLE = 'immediateDisable' - PERMANENT = 'permanent' - - -# get event name from enum -EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} - - -class Events: - def __init__(self): - self.events: list[int] = [] - self.static_events: list[int] = [] - self.event_counters = dict.fromkeys(EVENTS.keys(), 0) - - @property - def names(self) -> list[int]: - return self.events - - def __len__(self) -> int: - return len(self.events) - - def add(self, event_name: int, static: bool=False) -> None: - if static: - bisect.insort(self.static_events, event_name) - bisect.insort(self.events, event_name) - - def clear(self) -> None: - self.event_counters = {k: (v + 1 if k in self.events else 0) for k, v in self.event_counters.items()} - self.events = self.static_events.copy() - - def contains(self, event_type: str) -> bool: - return any(event_type in EVENTS.get(e, {}) for e in self.events) - - def create_alerts(self, event_types: list[str], callback_args=None): - if callback_args is None: - callback_args = [] - - ret = [] - for e in self.events: - types = EVENTS[e].keys() - for et in event_types: - if et in types: - alert = EVENTS[e][et] - if not isinstance(alert, Alert): - alert = alert(*callback_args) - - if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay: - alert.alert_type = f"{EVENT_NAME[e]}/{et}" - alert.event_type = et - ret.append(alert) - return ret - - def add_from_msg(self, events): - for e in events: - bisect.insort(self.events, e.name.raw) - - def to_msg(self): - ret = [] - for event_name in self.events: - event = car.OnroadEvent.new_message() - event.name = event_name - for event_type in EVENTS.get(event_name, {}): - setattr(event, event_type, True) - ret.append(event) - return ret - - -class Alert: - def __init__(self, - alert_text_1: str, - alert_text_2: str, - alert_status: log.SelfdriveState.AlertStatus, - alert_size: log.SelfdriveState.AlertSize, - priority: Priority, - visual_alert: car.CarControl.HUDControl.VisualAlert, - audible_alert: car.CarControl.HUDControl.AudibleAlert, - duration: float, - creation_delay: float = 0.): - - self.alert_text_1 = alert_text_1 - self.alert_text_2 = alert_text_2 - self.alert_status = alert_status - self.alert_size = alert_size - self.priority = priority - self.visual_alert = visual_alert - self.audible_alert = audible_alert - - self.duration = int(duration / DT_CTRL) - - self.creation_delay = creation_delay - - self.alert_type = "" - self.event_type: str | None = None - - def __str__(self) -> str: - return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" - - def __gt__(self, alert2) -> bool: - if not isinstance(alert2, Alert): - return False - return self.priority > alert2.priority - - -class NoEntryAlert(Alert): - def __init__(self, alert_text_2: str, - alert_text_1: str = "openpilot Unavailable", - visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none): - super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, - AlertSize.mid, Priority.LOW, visual_alert, - AudibleAlert.refuse, 3.) - - -class SoftDisableAlert(Alert): - def __init__(self, alert_text_2: str): - super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, - AlertStatus.userPrompt, AlertSize.full, - Priority.MID, VisualAlert.steerRequired, - AudibleAlert.warningSoft, 2.), - - -# less harsh version of SoftDisable, where the condition is user-triggered -class UserSoftDisableAlert(SoftDisableAlert): - def __init__(self, alert_text_2: str): - super().__init__(alert_text_2), - self.alert_text_1 = "openpilot will disengage" - - -class ImmediateDisableAlert(Alert): - def __init__(self, alert_text_2: str): - super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2, - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.steerRequired, - AudibleAlert.warningImmediate, 4.), - - -class EngagementAlert(Alert): - def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert): - super().__init__("", "", - AlertStatus.normal, AlertSize.none, - Priority.MID, VisualAlert.none, - audible_alert, .2), - - -class NormalPermanentAlert(Alert): - def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Priority = Priority.LOWER, creation_delay: float = 0.): - super().__init__(alert_text_1, alert_text_2, - AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small, - priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay=creation_delay), - - -class StartupAlert(Alert): - def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal): - super().__init__(alert_text_1, alert_text_2, - alert_status, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 5.), - - -# ********** helper functions ********** -def get_display_speed(speed_ms: float, metric: bool) -> str: - speed = int(round(speed_ms * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))) - unit = 'km/h' if metric else 'mph' - return f"{speed} {unit}" - - -# ********** alert callback functions ********** - -AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], Alert] - - -def soft_disable_alert(alert_text_2: str) -> AlertCallbackType: - def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if soft_disable_time < int(0.5 / DT_CTRL): - return ImmediateDisableAlert(alert_text_2) - return SoftDisableAlert(alert_text_2) - return func - -def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType: - def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if soft_disable_time < int(0.5 / DT_CTRL): - return ImmediateDisableAlert(alert_text_2) - return UserSoftDisableAlert(alert_text_2) - return func - -def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - branch = get_short_branch() # Ensure get_short_branch is cached to avoid lags on startup - if "REPLAY" in os.environ: - branch = "replay" - - return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt) - -def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage") - - -def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return Alert( - f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4) - - -def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - first_word = 'Recalibration' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibration' - return Alert( - f"{first_word} in Progress: {sm['liveCalibration'].calPerc:.0f}%", - f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", - AlertStatus.normal, AlertSize.mid, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) - - -# *** debug alerts *** - -def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - full_perc = round(100. - sm['deviceState'].freeSpacePercent) - return NormalPermanentAlert("Out of Storage", f"{full_perc}% full") - - -def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan - err = CS.vEgo - mdl - msg = f"Speed Error: {err:.1f} m/s" - return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid") - - -def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] - msg = ', '.join(not_running) - return NoEntryAlert(msg, alert_text_1="Process Not Running") - - -def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])] - msg = ', '.join(bs[:4]) # can't fit too many on one line - return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes") - - -def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') - bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] - return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) - - -def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - rpy = sm['liveCalibration'].rpyCalib - yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) - pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) - angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)" - return NormalPermanentAlert("Calibration Invalid", angles) - - -def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - cpu = max(sm['deviceState'].cpuTempC, default=0.) - gpu = max(sm['deviceState'].gpuTempC, default=0.) - temp = max((cpu, gpu, sm['deviceState'].memoryTempC)) - return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C") - - -def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used") - - -def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - x = max(sm['deviceState'].cpuUsagePercent, default=0.) - return NormalPermanentAlert("High CPU Usage", f"{x}% used") - - -def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped") - - -def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - text = "Enable Adaptive Cruise to Engage" - if CP.carName == "honda": - text = "Enable Main Switch to Engage" - return NoEntryAlert(text) - - -def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - axes = sm['testJoystick'].axes - gb, steer = list(axes)[:2] if len(axes) else (0., 0.) - vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" - return NormalPermanentAlert("Joystick Mode", vals) - -def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - personality = str(personality).title() - return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) - - - -EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { - # ********** events with no alerts ********** - - EventName.stockFcw: {}, - EventName.actuatorsApiUnavailable: {}, - - # ********** events only containing alerts displayed in all states ********** - - EventName.joystickDebug: { - ET.WARNING: joystick_alert, - ET.PERMANENT: NormalPermanentAlert("Joystick Mode"), - }, - - EventName.controlsInitializing: { - ET.NO_ENTRY: NoEntryAlert("System Initializing"), - }, - - EventName.startup: { - ET.PERMANENT: StartupAlert("Be ready to take over at any time") - }, - - EventName.startupMaster: { - ET.PERMANENT: startup_master_alert, - }, - - # Car is recognized, but marked as dashcam only - EventName.startupNoControl: { - ET.PERMANENT: StartupAlert("Dashcam mode"), - ET.NO_ENTRY: NoEntryAlert("Dashcam mode"), - }, - - # Car is not recognized - EventName.startupNoCar: { - ET.PERMANENT: StartupAlert("Dashcam mode for unsupported car"), - }, - - EventName.startupNoFw: { - ET.PERMANENT: StartupAlert("Car Unrecognized", - "Check comma power connections", - alert_status=AlertStatus.userPrompt), - }, - - EventName.dashcamMode: { - ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", - priority=Priority.LOWEST), - }, - - EventName.invalidLkasSetting: { - ET.PERMANENT: NormalPermanentAlert("Stock LKAS is on", - "Turn off stock LKAS to engage"), - }, - - EventName.cruiseMismatch: { - #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"), - }, - - # openpilot doesn't recognize the car. This switches openpilot into a - # read-only mode. This can be solved by adding your fingerprint. - # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information - EventName.carUnrecognized: { - ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", - "Car Unrecognized", - priority=Priority.LOWEST), - }, - - EventName.stockAeb: { - ET.PERMANENT: Alert( - "BRAKE!", - "Stock AEB: Risk of Collision", - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.), - ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"), - }, - - EventName.fcw: { - ET.PERMANENT: Alert( - "BRAKE!", - "Risk of Collision", - AlertStatus.critical, AlertSize.full, - Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.), - }, - - EventName.ldw: { - ET.PERMANENT: Alert( - "Lane Departure Detected", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.), - }, - - # ********** events only containing alerts that display while engaged ********** - - EventName.steerTempUnavailableSilent: { - ET.WARNING: Alert( - "Steering Temporarily Unavailable", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), - }, - - EventName.preDriverDistracted: { - ET.PERMANENT: Alert( - "Pay Attention", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.promptDriverDistracted: { - ET.PERMANENT: Alert( - "Pay Attention", - "Driver Distracted", - AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), - }, - - EventName.driverDistracted: { - ET.PERMANENT: Alert( - "DISENGAGE IMMEDIATELY", - "Driver Distracted", - AlertStatus.critical, AlertSize.full, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), - }, - - EventName.preDriverUnresponsive: { - ET.PERMANENT: Alert( - "Touch Steering Wheel: No Face Detected", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1), - }, - - EventName.promptDriverUnresponsive: { - ET.PERMANENT: Alert( - "Touch Steering Wheel", - "Driver Unresponsive", - AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), - }, - - EventName.driverUnresponsive: { - ET.PERMANENT: Alert( - "DISENGAGE IMMEDIATELY", - "Driver Unresponsive", - AlertStatus.critical, AlertSize.full, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), - }, - - EventName.manualRestart: { - ET.WARNING: Alert( - "TAKE CONTROL", - "Resume Driving Manually", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), - }, - - EventName.resumeRequired: { - ET.WARNING: Alert( - "Press Resume to Exit Standstill", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), - }, - - EventName.belowSteerSpeed: { - ET.WARNING: below_steer_speed_alert, - }, - - EventName.preLaneChangeLeft: { - ET.WARNING: Alert( - "Steer Left to Start Lane Change Once Safe", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.preLaneChangeRight: { - ET.WARNING: Alert( - "Steer Right to Start Lane Change Once Safe", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.laneChangeBlocked: { - ET.WARNING: Alert( - "Car Detected in Blindspot", - "", - AlertStatus.userPrompt, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), - }, - - EventName.laneChange: { - ET.WARNING: Alert( - "Changing Lanes", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.steerSaturated: { - ET.WARNING: Alert( - "Take Control", - "Turn Exceeds Steering Limit", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.), - }, - - # Thrown when the fan is driven at >50% but is not rotating - EventName.fanMalfunction: { - ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Likely Hardware Issue"), - }, - - # Camera is not outputting frames - EventName.cameraMalfunction: { - ET.PERMANENT: camera_malfunction_alert, - ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"), - ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"), - }, - # Camera framerate too low - EventName.cameraFrameRate: { - ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"), - ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"), - ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"), - }, - - # Unused - - EventName.locationdTemporaryError: { - ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"), - ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"), - }, - - EventName.locationdPermanentError: { - ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"), - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"), - ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"), - }, - - # openpilot tries to learn certain parameters about your car by observing - # how the car behaves to steering inputs from both human and openpilot driving. - # This includes: - # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle - # - tire stiffness: how much grip your tires have - # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight - # This alert is thrown when any of these values exceed a sanity check. This can be caused by - # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub - EventName.paramsdTemporaryError: { - ET.NO_ENTRY: NoEntryAlert("paramsd Temporary Error"), - ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"), - }, - - EventName.paramsdPermanentError: { - ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"), - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"), - ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"), - }, - - # ********** events that affect controls state transitions ********** - - EventName.pcmEnable: { - ET.ENABLE: EngagementAlert(AudibleAlert.engage), - }, - - EventName.buttonEnable: { - ET.ENABLE: EngagementAlert(AudibleAlert.engage), - }, - - EventName.pcmDisable: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - }, - - EventName.buttonCancel: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"), - }, - - EventName.brakeHold: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"), - }, - - EventName.parkBrake: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"), - }, - - EventName.pedalPressed: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Pedal Pressed", - visual_alert=VisualAlert.brakePressed), - }, - - EventName.preEnableStandstill: { - ET.PRE_ENABLE: Alert( - "Release Brake to Engage", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.), - }, - - EventName.gasPressedOverride: { - ET.OVERRIDE_LONGITUDINAL: Alert( - "", - "", - AlertStatus.normal, AlertSize.none, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.steerOverride: { - ET.OVERRIDE_LATERAL: Alert( - "", - "", - AlertStatus.normal, AlertSize.none, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), - }, - - EventName.wrongCarMode: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: wrong_car_mode_alert, - }, - - EventName.resumeBlocked: { - ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"), - }, - - EventName.wrongCruiseMode: { - ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), - ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"), - }, - - EventName.steerTempUnavailable: { - ET.SOFT_DISABLE: soft_disable_alert("Steering Temporarily Unavailable"), - ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), - }, - - EventName.steerTimeLimit: { - ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"), - ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"), - }, - - EventName.outOfSpace: { - ET.PERMANENT: out_of_space_alert, - ET.NO_ENTRY: NoEntryAlert("Out of Storage"), - }, - - EventName.belowEngageSpeed: { - ET.NO_ENTRY: below_engage_speed_alert, - }, - - EventName.sensorDataInvalid: { - ET.PERMANENT: Alert( - "Sensor Data Invalid", - "Possible Hardware Issue", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.), - ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"), - ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), - }, - - EventName.noGps: { - ET.PERMANENT: Alert( - "Poor GPS reception", - "Ensure device has a clear view of the sky", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=600.) - }, - - EventName.soundsUnavailable: { - ET.PERMANENT: NormalPermanentAlert("Speaker not found", "Reboot your Device"), - ET.NO_ENTRY: NoEntryAlert("Speaker not found"), - }, - - EventName.tooDistracted: { - ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), - }, - - EventName.overheat: { - ET.PERMANENT: overheat_alert, - ET.SOFT_DISABLE: soft_disable_alert("System Overheated"), - ET.NO_ENTRY: NoEntryAlert("System Overheated"), - }, - - EventName.wrongGear: { - ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"), - ET.NO_ENTRY: NoEntryAlert("Gear not D"), - }, - - # This alert is thrown when the calibration angles are outside of the acceptable range. - # For example if the device is pointed too much to the left or the right. - # Usually this can only be solved by removing the mount from the windshield completely, - # and attaching while making sure the device is pointed straight forward and is level. - # See https://comma.ai/setup for more information - EventName.calibrationInvalid: { - ET.PERMANENT: calibration_invalid_alert, - ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"), - ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), - }, - - EventName.calibrationIncomplete: { - ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), - ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), - }, - - EventName.calibrationRecalibrating: { - ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"), - ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"), - }, - - EventName.doorOpen: { - ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), - ET.NO_ENTRY: NoEntryAlert("Door Open"), - }, - - EventName.seatbeltNotLatched: { - ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"), - ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), - }, - - EventName.espDisabled: { - ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Disabled"), - ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Disabled"), - }, - - EventName.lowBattery: { - ET.SOFT_DISABLE: soft_disable_alert("Low Battery"), - ET.NO_ENTRY: NoEntryAlert("Low Battery"), - }, - - # Different openpilot services communicate between each other at a certain - # interval. If communication does not follow the regular schedule this alert - # is thrown. This can mean a service crashed, did not broadcast a message for - # ten times the regular interval, or the average interval is more than 10% too high. - EventName.commIssue: { - ET.SOFT_DISABLE: soft_disable_alert("Communication Issue Between Processes"), - ET.NO_ENTRY: comm_issue_alert, - }, - EventName.commIssueAvgFreq: { - ET.SOFT_DISABLE: soft_disable_alert("Low Communication Rate Between Processes"), - ET.NO_ENTRY: NoEntryAlert("Low Communication Rate Between Processes"), - }, - - EventName.controlsdLagging: { - ET.SOFT_DISABLE: soft_disable_alert("Controls Lagging"), - ET.NO_ENTRY: NoEntryAlert("Controls Process Lagging: Reboot Your Device"), - }, - - # Thrown when manager detects a service exited unexpectedly while driving - EventName.processNotRunning: { - ET.NO_ENTRY: process_not_running_alert, - ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"), - }, - - EventName.radarFault: { - ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"), - ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), - }, - - # Every frame from the camera should be processed by the model. If modeld - # is not processing frames fast enough they have to be dropped. This alert is - # thrown when over 20% of frames are dropped. - EventName.modeldLagging: { - ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"), - ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"), - ET.PERMANENT: modeld_lagging_alert, - }, - - # Besides predicting the path, lane lines and lead car data the model also - # predicts the current velocity and rotation speed of the car. If the model is - # very uncertain about the current velocity while the car is moving, this - # usually means the model has trouble understanding the scene. This is used - # as a heuristic to warn the driver. - EventName.posenetInvalid: { - ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"), - ET.NO_ENTRY: posenet_invalid_alert, - }, - - # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we - # alert the driver the device might have fallen from the windshield. - EventName.deviceFalling: { - ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"), - ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), - }, - - EventName.lowMemory: { - ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"), - ET.PERMANENT: low_memory_alert, - ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"), - }, - - EventName.highCpuUsage: { - #ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"), - #ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"), - ET.NO_ENTRY: high_cpu_usage_alert, - }, - - EventName.accFaulted: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"), - ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), - ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), - }, - - EventName.espActive: { - ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Active"), - ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Active"), - }, - - EventName.controlsMismatch: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"), - ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), - }, - - EventName.roadCameraError: { - ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road", - duration=1., - creation_delay=30.), - }, - - EventName.wideRoadCameraError: { - ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road Fisheye", - duration=1., - creation_delay=30.), - }, - - EventName.driverCameraError: { - ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Driver", - duration=1., - creation_delay=30.), - }, - - # Sometimes the USB stack on the device can get into a bad state - # causing the connection to the panda to be lost - EventName.usbError: { - ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"), - ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device", ""), - ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"), - }, - - # This alert can be thrown for the following reasons: - # - No CAN data received at all - # - CAN data is received, but some message are not received at the right frequency - # If you're not writing a new car port, this is usually cause by faulty wiring - EventName.canError: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Error"), - ET.PERMANENT: Alert( - "CAN Error: Check Connections", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), - ET.NO_ENTRY: NoEntryAlert("CAN Error: Check Connections"), - }, - - EventName.canBusMissing: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"), - ET.PERMANENT: Alert( - "CAN Bus Disconnected: Likely Faulty Cable", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), - ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"), - }, - - EventName.steerUnavailable: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"), - ET.PERMANENT: NormalPermanentAlert("LKAS Fault: Restart the car to engage"), - ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"), - }, - - EventName.reverseGear: { - ET.PERMANENT: Alert( - "Reverse\nGear", - "", - AlertStatus.normal, AlertSize.full, - Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), - ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"), - ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), - }, - - # On cars that use stock ACC the car can decide to cancel ACC for various reasons. - # When this happens we can no long control the car so the user needs to be warned immediately. - EventName.cruiseDisabled: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"), - }, - - # When the relay in the harness box opens the CAN bus between the LKAS camera - # and the rest of the car is separated. When messages from the LKAS camera - # are received on the car side this usually means the relay hasn't opened correctly - # and this alert is thrown. - EventName.relayMalfunction: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"), - ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"), - ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"), - }, - - EventName.speedTooLow: { - ET.IMMEDIATE_DISABLE: Alert( - "openpilot Canceled", - "Speed too low", - AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), - }, - - # When the car is driving faster than most cars in the training data, the model outputs can be unpredictable. - EventName.speedTooHigh: { - ET.WARNING: Alert( - "Speed Too High", - "Model uncertain at this speed", - AlertStatus.userPrompt, AlertSize.mid, - Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.), - ET.NO_ENTRY: NoEntryAlert("Slow down to engage"), - }, - - EventName.lowSpeedLockout: { - ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), - ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), - }, - - EventName.lkasDisabled: { - ET.PERMANENT: NormalPermanentAlert("LKAS Disabled: Enable LKAS to engage"), - ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), - }, - - EventName.vehicleSensorsInvalid: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"), - ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"), - ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), - }, - - EventName.personalityChanged: { - ET.WARNING: personality_changed_alert, - }, - -} - - -if __name__ == '__main__': - # print all alerts by type and priority - from cereal.services import SERVICE_LIST - from collections import defaultdict - - event_names = {v: k for k, v in EventName.schema.enumerants.items()} - alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list)) - - CP = car.CarParams.new_message() - CS = car.CarState.new_message() - sm = messaging.SubMaster(list(SERVICE_LIST.keys())) - - for i, alerts in EVENTS.items(): - for et, alert in alerts.items(): - if callable(alert): - alert = alert(CP, CS, sm, False, 1, log.LongitudinalPersonality.standard) - alerts_by_type[et][alert.priority].append(event_names[i]) - - all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {} - for et, priority_alerts in alerts_by_type.items(): - all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True) - - for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]): - print(f"**** {status} ****") - for p, alert_list in evs: - print(f" {repr(p)}:") - print(" ", ', '.join(alert_list), "\n") diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py deleted file mode 100644 index 8b9c18a9d48206..00000000000000 --- a/selfdrive/controls/lib/tests/test_alertmanager.py +++ /dev/null @@ -1,39 +0,0 @@ -import random - -from openpilot.selfdrive.controls.lib.events import Alert, EVENTS -from openpilot.selfdrive.controls.lib.alertmanager import AlertManager - - -class TestAlertManager: - - def test_duration(self): - """ - Enforce that an alert lasts for max(alert duration, duration the alert is added) - """ - for duration in range(1, 100): - alert = None - while not isinstance(alert, Alert): - event = random.choice([e for e in EVENTS.values() if len(e)]) - alert = random.choice(list(event.values())) - - alert.duration = duration - - # check two cases: - # - alert is added to AM for <= the alert's duration - # - alert is added to AM for > alert's duration - for greater in (True, False): - if greater: - add_duration = duration + random.randint(1, 10) - else: - add_duration = random.randint(1, duration) - show_duration = max(duration, add_duration) - - AM = AlertManager() - for frame in range(duration+10): - if frame < add_duration: - AM.add_many(frame, [alert, ]) - current_alert = AM.process_alerts(frame, {}) - - shown = current_alert is not None - should_show = frame <= show_duration - assert shown == should_show, f"{frame=} {add_duration=} {duration=}" diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py deleted file mode 100644 index 52d71dc22dd68b..00000000000000 --- a/selfdrive/controls/tests/test_alerts.py +++ /dev/null @@ -1,130 +0,0 @@ -import copy -import json -import os -import random -from PIL import Image, ImageDraw, ImageFont - -from cereal import log, car -from cereal.messaging import SubMaster -from openpilot.common.basedir import BASEDIR -from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET -from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert -from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS - -AlertSize = log.SelfdriveState.AlertSize - -OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json") - -# TODO: add callback alerts -ALERTS = [] -for event_types in EVENTS.values(): - for alert in event_types.values(): - ALERTS.append(alert) - - -class TestAlerts: - - @classmethod - def setup_class(cls): - with open(OFFROAD_ALERTS_PATH) as f: - cls.offroad_alerts = json.loads(f.read()) - - # Create fake objects for callback - cls.CS = car.CarState.new_message() - cls.CP = car.CarParams.new_message() - cfg = [c for c in CONFIGS if c.proc_name == 'controlsd'][0] - cls.sm = SubMaster(cfg.pubs) - - def test_events_defined(self): - # Ensure all events in capnp schema are defined in events.py - events = car.OnroadEvent.EventName.schema.enumerants - - for name, e in events.items(): - if not name.endswith("DEPRECATED"): - fail_msg = "%s @%d not in EVENTS" % (name, e) - assert e in EVENTS.keys(), fail_msg - - # ensure alert text doesn't exceed allowed width - def test_alert_text_length(self): - font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts") - regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") - bold_font_path = os.path.join(font_path, "Inter-Bold.ttf") - semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") - - max_text_width = 2160 - 300 # full screen width is usable, minus sidebar - draw = ImageDraw.Draw(Image.new('RGB', (0, 0))) - - fonts = { - AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)], - AlertSize.mid: [ImageFont.truetype(bold_font_path, 88), - ImageFont.truetype(regular_font_path, 66)], - } - - for alert in ALERTS: - if not isinstance(alert, Alert): - alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard) - - # for full size alerts, both text fields wrap the text, - # so it's unlikely that they would go past the max width - if alert.alert_size in (AlertSize.none, AlertSize.full): - continue - - for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]): - if i >= len(fonts[alert.alert_size]): - break - - font = fonts[alert.alert_size][i] - left, _, right, _ = draw.textbbox((0, 0), txt, font) - width = right - left - msg = f"type: {alert.alert_type} msg: {txt}" - assert width <= max_text_width, msg - - def test_alert_sanity_check(self): - for event_types in EVENTS.values(): - for event_type, a in event_types.items(): - # TODO: add callback alerts - if not isinstance(a, Alert): - continue - - if a.alert_size == AlertSize.none: - assert len(a.alert_text_1) == 0 - assert len(a.alert_text_2) == 0 - elif a.alert_size == AlertSize.small: - assert len(a.alert_text_1) > 0 - assert len(a.alert_text_2) == 0 - elif a.alert_size == AlertSize.mid: - assert len(a.alert_text_1) > 0 - assert len(a.alert_text_2) > 0 - else: - assert len(a.alert_text_1) > 0 - - assert a.duration >= 0. - - if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE): - assert a.creation_delay == 0. - - def test_offroad_alerts(self): - params = Params() - for a in self.offroad_alerts: - # set the alert - alert = copy.copy(self.offroad_alerts[a]) - set_offroad_alert(a, True) - alert['extra'] = '' - assert json.dumps(alert) == params.get(a, encoding='utf8') - - # then delete it - set_offroad_alert(a, False) - assert params.get(a) is None - - def test_offroad_alerts_extra_text(self): - params = Params() - for i in range(50): - # set the alert - a = random.choice(list(self.offroad_alerts)) - alert = self.offroad_alerts[a] - set_offroad_alert(a, True, extra_text="a"*i) - - written_alert = json.loads(params.get(a, encoding='utf8')) - assert "a"*i == written_alert['extra'] - assert alert["text"] == written_alert['text'] diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py deleted file mode 100644 index 8f451a49bc1d26..00000000000000 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ /dev/null @@ -1,151 +0,0 @@ -import pytest -import itertools -import numpy as np - -from parameterized import parameterized_class -from cereal import log -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT -from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver - -ButtonEvent = car.CarState.ButtonEvent -ButtonType = car.CarState.ButtonEvent.Type - - -def run_cruise_simulation(cruise, e2e, personality, t_end=20.): - man = Maneuver( - '', - duration=t_end, - initial_speed=max(cruise - 1., 0.0), - lead_relevancy=True, - initial_distance_lead=100, - cruise_values=[cruise], - prob_lead_values=[0.0], - breakpoints=[0.], - e2e=e2e, - personality=personality, - ) - valid, output = man.evaluate() - assert valid - return output[-1, 3] - - -@parameterized_class(("e2e", "personality", "speed"), itertools.product( - [True, False], # e2e - log.LongitudinalPersonality.schema.enumerants, # personality - [5,35])) # speed -class TestCruiseSpeed: - def test_cruise_speed(self): - print(f'Testing {self.speed} m/s') - cruise_speed = float(self.speed) - - simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality) - assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' - - -# TODO: test pcmCruise -@parameterized_class(('pcm_cruise',), [(False,)]) -class TestVCruiseHelper: - def setup_method(self): - self.CP = car.CarParams(pcmCruise=self.pcm_cruise) - self.v_cruise_helper = VCruiseHelper(self.CP) - self.reset_cruise_speed_state() - - def reset_cruise_speed_state(self): - # Two resets previous cruise speed - for _ in range(2): - self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) - - def enable(self, v_ego, experimental_mode): - # Simulates user pressing set with a current speed - self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode) - - def test_adjust_speed(self): - """ - Asserts speed changes on falling edges of buttons. - """ - - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - - for btn in (ButtonType.accelCruise, ButtonType.decelCruise): - for pressed in (True, False): - CS = car.CarState(cruiseState={"available": True}) - CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)] - - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) - assert pressed == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) - - def test_rising_edge_enable(self): - """ - Some car interfaces may enable on rising edge of a button, - ensure we don't adjust speed if enabled changes mid-press. - """ - - # NOTE: enabled is always one frame behind the result from button press in controlsd - for enabled, pressed in ((False, False), - (False, True), - (True, False)): - CS = car.CarState(cruiseState={"available": True}) - CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) - if pressed: - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - - # Expected diff on enabling. Speed should not change on falling edge of pressed - assert not pressed == self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last - - def test_resume_in_standstill(self): - """ - Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. - """ - - self.enable(0, False) - - for standstill in (True, False): - for pressed in (True, False): - CS = car.CarState(cruiseState={"available": True, "standstill": standstill}) - CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) - - # speed should only update if not at standstill and button falling edge - should_equal = standstill or pressed - assert should_equal == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) - - def test_set_gas_pressed(self): - """ - Asserts pressing set while enabled with gas pressed sets - the speed to the maximum of vEgo and current cruise speed. - """ - - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - - # first decrement speed, then perform gas pressed logic - expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT - expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH) # clip to min of vEgo - expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)) - - CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True}) - CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) - - # TODO: fix skipping first run due to enabled on rising edge exception - if v_ego == 0.0: - continue - assert expected_v_cruise_kph == self.v_cruise_helper.v_cruise_kph - - def test_initialize_v_cruise(self): - """ - Asserts allowed cruise speeds on enabling with SET. - """ - - for experimental_mode in (True, False): - for v_ego in np.linspace(0, 100, 101): - self.reset_cruise_speed_state() - assert not self.v_cruise_helper.v_cruise_initialized - - self.enable(float(v_ego), experimental_mode) - assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX - assert self.v_cruise_helper.v_cruise_initialized diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py deleted file mode 100644 index 077d4ae93aa80e..00000000000000 --- a/selfdrive/controls/tests/test_startup.py +++ /dev/null @@ -1,121 +0,0 @@ -import os -from parameterized import parameterized - -from cereal import log, car -import cereal.messaging as messaging -from opendbc.car.fingerprints import _FINGERPRINTS -from opendbc.car.toyota.values import CAR as TOYOTA -from opendbc.car.mazda.values import CAR as MAZDA -from openpilot.common.params import Params -from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp -from openpilot.selfdrive.controls.lib.events import EVENT_NAME -from openpilot.system.manager.process_config import managed_processes - -EventName = car.OnroadEvent.EventName -Ecu = car.CarParams.Ecu - -COROLLA_FW_VERSIONS = [ - (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'), - (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'), - (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'), - (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'), - (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'), -] -COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')] -COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1] - -CX5_FW_VERSIONS = [ - (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), - (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), -] - - -@parameterized.expand([ - # TODO: test EventName.startup for release branches - - # officially supported car - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), - - # dashcamOnly car - (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), - (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), - - # unrecognized car with no fw - (EventName.startupNoFw, None, None, ""), - (EventName.startupNoFw, None, None, ""), - - # unrecognized car - (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), - (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), - - # fuzzy match - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), - (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), -]) -def test_startup_alert(expected_event, car_model, fw_versions, brand): - controls_sock = messaging.sub_sock("selfdriveState") - pm = messaging.PubMaster(['can', 'pandaStates']) - - params = Params() - params.put_bool("OpenpilotEnabledToggle", True) - - # Build capnn version of FW array - if fw_versions is not None: - car_fw = [] - cp = car.CarParams.new_message() - for ecu, addr, subaddress, version in fw_versions: - f = car.CarParams.CarFw.new_message() - f.ecu = ecu - f.address = addr - f.fwVersion = version - f.brand = brand - - if subaddress is not None: - f.subAddress = subaddress - - car_fw.append(f) - cp.carVin = "1" * 17 - cp.carFw = car_fw - params.put("CarParamsCache", cp.to_bytes()) - else: - os.environ['SKIP_FW_QUERY'] = '1' - - managed_processes['controlsd'].start() - managed_processes['card'].start() - - assert pm.wait_for_readers_to_update('can', 5) - pm.send('can', can_list_to_can_capnp([[0, b"", 0]])) - - assert pm.wait_for_readers_to_update('pandaStates', 5) - msg = messaging.new_message('pandaStates', 1) - msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno - pm.send('pandaStates', msg) - - # fingerprint - if (car_model is None) or (fw_versions is not None): - finger = {addr: 1 for addr in range(1, 100)} - else: - finger = _FINGERPRINTS[car_model][0] - - msgs = [[addr, b'\x00'*length, 0] for addr, length in finger.items()] - for _ in range(1000): - # card waits for pandad to echo back that it has changed the multiplexing mode - if not params.get_bool("ObdMultiplexingChanged"): - params.put_bool("ObdMultiplexingChanged", True) - - pm.send('can', can_list_to_can_capnp(msgs)) - assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}" - - ctrls = messaging.drain_sock(controls_sock) - if len(ctrls): - event_name = ctrls[0].selfdriveState.alertType.split("/")[0] - assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}" - break - else: - raise Exception(f"failed to fingerprint {car_model}") diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py deleted file mode 100644 index 3926b88eb27382..00000000000000 --- a/selfdrive/controls/tests/test_state_machine.py +++ /dev/null @@ -1,102 +0,0 @@ -from cereal import car, log -from opendbc.car.car_helpers import interfaces -from opendbc.car.mock.values import CAR as MOCK -from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME -from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \ - AudibleAlert, EVENTS - -State = log.SelfdriveState.OpenpilotState - -# The event types that maintain the current state -MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,), - State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)} -ALL_STATES = tuple(State.schema.enumerants.values()) -# The event types checked in DISABLED section of state machine -ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL) - - -def make_event(event_types): - event = {} - for ev in event_types: - event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW, - VisualAlert.none, AudibleAlert.none, 1.) - EVENTS[0] = event - return 0 - - -class TestStateMachine: - - def setup_method(self): - CarInterface, CarController, CarState = interfaces[MOCK.MOCK] - CP = CarInterface.get_non_essential_params(MOCK.MOCK) - CI = CarInterface(CP, CarController, CarState) - - self.controlsd = Controls(CI=CI) - self.controlsd.events = Events() - self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) - self.CS = car.CarState() - - def test_immediate_disable(self): - for state in ALL_STATES: - for et in MAINTAIN_STATES[state]: - self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE])) - self.controlsd.state = state - self.controlsd.state_transition(self.CS) - assert State.disabled == self.controlsd.state - self.controlsd.events.clear() - - def test_user_disable(self): - for state in ALL_STATES: - for et in MAINTAIN_STATES[state]: - self.controlsd.events.add(make_event([et, ET.USER_DISABLE])) - self.controlsd.state = state - self.controlsd.state_transition(self.CS) - assert State.disabled == self.controlsd.state - self.controlsd.events.clear() - - def test_soft_disable(self): - for state in ALL_STATES: - if state == State.preEnabled: # preEnabled considers NO_ENTRY instead - continue - for et in MAINTAIN_STATES[state]: - self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE])) - self.controlsd.state = state - self.controlsd.state_transition(self.CS) - assert self.controlsd.state == State.disabled if state == State.disabled else State.softDisabling - self.controlsd.events.clear() - - def test_soft_disable_timer(self): - self.controlsd.state = State.enabled - self.controlsd.events.add(make_event([ET.SOFT_DISABLE])) - self.controlsd.state_transition(self.CS) - for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): - assert self.controlsd.state == State.softDisabling - self.controlsd.state_transition(self.CS) - - assert self.controlsd.state == State.disabled - - def test_no_entry(self): - # Make sure noEntry keeps us disabled - for et in ENABLE_EVENT_TYPES: - self.controlsd.events.add(make_event([ET.NO_ENTRY, et])) - self.controlsd.state_transition(self.CS) - assert self.controlsd.state == State.disabled - self.controlsd.events.clear() - - def test_no_entry_pre_enable(self): - # preEnabled with noEntry event - self.controlsd.state = State.preEnabled - self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) - self.controlsd.state_transition(self.CS) - assert self.controlsd.state == State.preEnabled - - def test_maintain_states(self): - # Given current state's event type, we should maintain state - for state in ALL_STATES: - for et in MAINTAIN_STATES[state]: - self.controlsd.state = state - self.controlsd.events.add(make_event([et])) - self.controlsd.state_transition(self.CS) - assert self.controlsd.state == state - self.controlsd.events.clear() diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc deleted file mode 100644 index e1e5cd1bb2ccb1..00000000000000 --- a/selfdrive/locationd/locationd.cc +++ /dev/null @@ -1,716 +0,0 @@ -#include "selfdrive/locationd/locationd.h" - -#include -#include - -#include -#include -#include - -using namespace EKFS; -using namespace Eigen; - -ExitHandler do_exit; -const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 -const double ROTATION_SANITY_CHECK = 10.0; // rad/s -const double TRANS_SANITY_CHECK = 200.0; // m/s -const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) -const double ALTITUDE_SANITY_CHECK = 10000; // m -const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad -const double SANE_GPS_UNCERTAINTY = 1500.0; // m -const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker -const double RESET_TRACKER_DECAY = 0.99995; -const double DECAY = 0.9993; // ~10 secs to resume after a bad input -const double MAX_FILTER_REWIND_TIME = 0.8; // s -const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; - -// TODO: GPS sensor time offsets are empirically calculated -// They should be replaced with synced time from a real clock -const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s -const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s -const float GPS_POS_STD_THRESHOLD = 50.0; -const float GPS_VEL_STD_THRESHOLD = 5.0; -const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; -const float GPS_POS_STD_RESET_THRESHOLD = 2.0; -const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; -const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; -const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; - -const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0"; - -template -static Vector floatlist2vector(const ListType& floatlist) { - Vector res(floatlist.size()); - for (int i = 0; i < floatlist.size(); i++) { - res[i] = floatlist[i]; - } - return res; -} - -template -static VectorXd float64list2vector(const ListType& floatlist) { - return floatlist2vector(floatlist); -} - -template -static VectorXf float32list2vector(const ListType& floatlist) { - return floatlist2vector(floatlist); -} - -static Vector4d quat2vector(const Quaterniond& quat) { - return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); -} - -static Quaterniond vector2quat(const VectorXd& vec) { - return Quaterniond(vec(0), vec(1), vec(2), vec(3)); -} - -static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { - meas.setX(val[0]); meas.setY(val[1]); meas.setZ(val[2]); - meas.setXStd(std[0]); meas.setYStd(std[1]); meas.setZStd(std[2]); - meas.setValid(valid); -} - -static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { - // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix - return ((rot_matrix * cov_in) * rot_matrix.transpose()); -} - -static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { - // Stds cannot be rotated like values, only covariances can be rotated - return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); -} - -Localizer::Localizer(LocalizerGnssSource gnss_source) { - this->kf = std::make_unique(); - this->reset_kalman(); - - this->calib = Vector3d(0.0, 0.0, 0.0); - this->device_from_calib = MatrixXdr::Identity(3, 3); - this->calib_from_device = MatrixXdr::Identity(3, 3); - - for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) { - this->posenet_stds.push_back(10.0); - } - - VectorXd ecef_pos = this->kf->get_x().segment(STATE_ECEF_POS_START); - this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); - this->configure_gnss_source(gnss_source); -} - -void Localizer::build_live_pose(cereal::LivePose::Builder& fix) { - VectorXd predicted_state = this->kf->get_x(); - MatrixXdr predicted_cov = this->kf->get_P(); - VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); - - VectorXd fix_ecef = predicted_state.segment(STATE_ECEF_POS_START); - ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; - VectorXd fix_ecef_std = predicted_std.segment(STATE_ECEF_POS_ERR_START); - VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); - VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); - VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); - VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); - MatrixXdr orientation_ecef_cov = predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); - MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); - - MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); - - VectorXd vel_device = device_from_ecef * vel_ecef; - VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); - MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); - condensed_cov.topLeftCorner() = - predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); - condensed_cov.topRightCorner() = - predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); - condensed_cov.bottomRightCorner() = - predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); - condensed_cov.bottomLeftCorner() = - predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); - VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size()); - H_input << device_from_ecef_eul, vel_ecef; - MatrixXdr HH = this->kf->H(H_input); - MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); - VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); - - VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); - VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); - VectorXd nextfix_ecef = fix_ecef + vel_ecef; - VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); - - VectorXd accDevice = predicted_state.segment(STATE_ACCELERATION_START); - VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); - - VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); - VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); - - // write measurements to msg - init_xyz_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); - init_xyz_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); - init_xyz_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); - init_xyz_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); - if (DEBUG) { - cereal::LivePose::FilterState::Builder filter_state_builder = fix.initFilterState(); - filter_state_builder.setValue(kj::arrayPtr(predicted_state.data(), predicted_state.size())); - filter_state_builder.setStd(kj::arrayPtr(predicted_std.data(), predicted_std.size())); - filter_state_builder.setValid(true); - } - - double old_mean = 0.0, new_mean = 0.0; - int i = 0; - for (double x : this->posenet_stds) { - if (i < POSENET_STD_HIST_HALF) { - old_mean += x; - } else { - new_mean += x; - } - i++; - } - old_mean /= POSENET_STD_HIST_HALF; - new_mean /= POSENET_STD_HIST_HALF; - // experimentally found these values, no false positives in 20k minutes of driving - bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); - fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); -} - -VectorXd Localizer::get_position_geodetic() { - VectorXd fix_ecef = this->kf->get_x().segment(STATE_ECEF_POS_START); - ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; - Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef); - return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt); -} - -VectorXd Localizer::get_state() { - return this->kf->get_x(); -} - -VectorXd Localizer::get_stdev() { - return this->kf->get_P().diagonal().array().sqrt(); -} - -bool Localizer::are_inputs_ok() { - return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; -} - -void Localizer::observation_timings_invalid_reset(){ - this->observation_timings_invalid = false; -} - -void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { - // TODO does not yet account for double sensor readings in the log - - // Ignore empty readings (e.g. in case the magnetometer had no data ready) - if (log.getTimestamp() == 0) { - return; - } - - double sensor_time = 1e-9 * log.getTimestamp(); - - // sensor time and log time should be close - if (std::abs(current_time - sensor_time) > 0.1) { - LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); - this->observation_timings_invalid = true; - return; - } else if (!this->is_timestamp_valid(sensor_time)) { - this->observation_timings_invalid = true; - return; - } - - // TODO: handle messages from two IMUs at the same time - if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { - return; - } - - // Gyro Uncalibrated - if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { - auto v = log.getGyroUncalibrated().getV(); - auto meas = Vector3d(-v[2], -v[1], -v[0]); - - VectorXd gyro_bias = this->kf->get_x().segment(STATE_GYRO_BIAS_START); - float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]); - float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1]; - bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold; - - if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); - this->observation_values_invalid["gyroscope"] *= DECAY; - } else { - this->observation_values_invalid["gyroscope"] += 1.0; - } - } - - // Accelerometer - if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { - auto v = log.getAcceleration().getV(); - - // TODO: reduce false positives and re-enable this check - // check if device fell, estimate 10 for g - // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving - // this->device_fell |= (float64list2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - - auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ACCEL_SANITY_CHECK) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); - this->observation_values_invalid["accelerometer"] *= DECAY; - } else { - this->observation_values_invalid["accelerometer"] += 1.0; - } - } -} - -void Localizer::input_fake_gps_observations(double current_time) { - // This is done to make sure that the error estimate of the position does not blow up - // when the filter is in no-gps mode - // Steps : first predict -> observe current obs with reasonable STD - this->kf->predict(current_time); - - VectorXd current_x = this->kf->get_x(); - VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START); - VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START); - const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov(); - const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov(); - - this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); -} - -void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { - bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); - bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); - bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); - bool gps_vel_insane = (float64list2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - - if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { - //this->gps_valid = false; - this->determine_gps_mode(current_time); - return; - } - - double sensor_time = current_time - sensor_time_offset; - - // Process message - //this->gps_valid = true; - this->gps_mode = true; - Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; - this->converter = std::make_unique(geodetic); - - VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); - VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; - float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); - MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); - MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); - - this->unix_timestamp_millis = log.getUnixTimestampMillis(); - double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); - - VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); - VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); - VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); - VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; - for (int i = 0; i < orientation_error.size(); i++) { - orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); - if (orientation_error(i) < 0.0) { - orientation_error(i) += 2.0 * M_PI; - } - orientation_error(i) -= M_PI; - } - VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); - - if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { - LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); - } else if (gps_est_error > 100.0) { - LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - } - - this->last_gps_msg = sensor_time; - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); -} - -void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { - - if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { - this->determine_gps_mode(current_time); - return; - } - - double sensor_time = log.getMeasTime() * 1e-9; - sensor_time -= this->gps_time_offset; - - auto ecef_pos_v = log.getPositionECEF().getValue(); - VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]); - - // indexed at 0 cause all std values are the same MAE - auto ecef_pos_std = log.getPositionECEF().getStd()[0]; - MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); - - auto ecef_vel_v = log.getVelocityECEF().getValue(); - VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); - - // indexed at 0 cause all std values are the same MAE - auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; - MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); - - double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); - - VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); - VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef); - - LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); - ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; - VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); - double bearing_rad = atan2(ned_vel[1], ned_vel[0]); - - VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); - VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; - for (int i = 0; i < orientation_error.size(); i++) { - orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); - if (orientation_error(i) < 0.0) { - orientation_error(i) += 2.0 * M_PI; - } - orientation_error(i) -= M_PI; - } - VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); - - if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { - this->determine_gps_mode(current_time); - return; - } - - // prevent jumping gnss measurements (covered lots, standstill...) - bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; - orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; - orientation_reset &= !this->standstill; - if (orientation_reset) { - this->orientation_reset_count++; - } else { - this->orientation_reset_count = 0; - } - - if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { - // always reset on first gps message and if the location is off but the accuracy is high - LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { - LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); - this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); - this->orientation_reset_count = 0; - } - - this->gps_mode = true; - this->last_gps_msg = sensor_time; - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); - this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); -} - -void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { - this->car_speed = std::abs(log.getVEgo()); - this->standstill = log.getStandstill(); - if (this->standstill) { - this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); - this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) }); - } -} - -void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { - VectorXd rot_device = this->device_from_calib * float64list2vector(log.getRot()); - VectorXd trans_device = this->device_from_calib * float64list2vector(log.getTrans()); - - if (!this->is_timestamp_valid(current_time)) { - this->observation_timings_invalid = true; - return; - } - - if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { - this->observation_values_invalid["cameraOdometry"] += 1.0; - return; - } - - VectorXd rot_calib_std = float64list2vector(log.getRotStd()); - VectorXd trans_calib_std = float64list2vector(log.getTransStd()); - - if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { - this->observation_values_invalid["cameraOdometry"] += 1.0; - return; - } - - if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { - this->observation_values_invalid["cameraOdometry"] += 1.0; - return; - } - - this->posenet_stds.pop_front(); - this->posenet_stds.push_back(trans_calib_std[0]); - - // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise - trans_calib_std *= 10.0; - rot_calib_std *= 10.0; - MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal(); - MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal(); - this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, - { rot_device }, { rot_device_cov }); - this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, - { trans_device }, { trans_device_cov }); - this->observation_values_invalid["cameraOdometry"] *= DECAY; - this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); -} - -void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { - if (!this->is_timestamp_valid(current_time)) { - this->observation_timings_invalid = true; - return; - } - - if (log.getRpyCalib().size() > 0) { - auto live_calib = float64list2vector(log.getRpyCalib()); - if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { - this->observation_values_invalid["liveCalibration"] += 1.0; - return; - } - - this->calib = live_calib; - this->device_from_calib = euler2rot(this->calib); - this->calib_from_device = this->device_from_calib.transpose(); - this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; - this->observation_values_invalid["liveCalibration"] *= DECAY; - } -} - -void Localizer::reset_kalman(double current_time) { - const VectorXd &init_x = this->kf->get_initial_x(); - const MatrixXdr &init_P = this->kf->get_initial_P(); - this->reset_kalman(current_time, init_x, init_P); -} - -void Localizer::finite_check(double current_time) { - bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); - if (!all_finite) { - LOGE("Non-finite values detected, kalman reset"); - this->reset_kalman(current_time); - } -} - -void Localizer::time_check(double current_time) { - if (std::isnan(this->last_reset_time)) { - this->last_reset_time = current_time; - } - if (std::isnan(this->first_valid_log_time)) { - this->first_valid_log_time = current_time; - } - double filter_time = this->kf->get_filter_time(); - bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); - if (big_time_gap) { - LOGE("Time gap of over 10s detected, kalman reset"); - this->reset_kalman(current_time); - } -} - -void Localizer::update_reset_tracker() { - // reset tracker is tuned to trigger when over 1reset/10s over 2min period - if (this->is_gps_ok()) { - this->reset_tracker *= RESET_TRACKER_DECAY; - } else { - this->reset_tracker = 0.0; - } -} - -void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) { - // too nonlinear to init on completely wrong - VectorXd current_x = this->kf->get_x(); - MatrixXdr current_P = this->kf->get_P(); - MatrixXdr init_P = this->kf->get_initial_P(); - const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P(); - int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); - - current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient; - current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel; - current_x.segment(STATE_ECEF_POS_START) = init_pos; - - init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); - init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); - init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal(); - init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, - STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal(); - - this->reset_kalman(current_time, current_x, init_P); -} - -void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) { - this->kf->init_state(init_x, init_P, current_time); - this->last_reset_time = current_time; - this->reset_tracker += 1.0; -} - -void Localizer::handle_msg_bytes(const char *data, const size_t size) { - AlignedBuffer aligned_buf; - - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size)); - cereal::Event::Reader event = cmsg.getRoot(); - - this->handle_msg(event); -} - -void Localizer::handle_msg(const cereal::Event::Reader& log) { - double t = log.getLogMonoTime() * 1e-9; - this->time_check(t); - if (log.isAccelerometer()) { - this->handle_sensor(t, log.getAccelerometer()); - } else if (log.isGyroscope()) { - this->handle_sensor(t, log.getGyroscope()); - } else if (log.isGpsLocation()) { - this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); - } else if (log.isGpsLocationExternal()) { - this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); - //} else if (log.isGnssMeasurements()) { - // this->handle_gnss(t, log.getGnssMeasurements()); - } else if (log.isCarState()) { - this->handle_car_state(t, log.getCarState()); - } else if (log.isCameraOdometry()) { - this->handle_cam_odo(t, log.getCameraOdometry()); - } else if (log.isLiveCalibration()) { - this->handle_live_calib(t, log.getLiveCalibration()); - } - this->finite_check(); - this->update_reset_tracker(); -} - -void Localizer::build_pose_message( - MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) { - cereal::Event::Builder evt = msg_builder.initEvent(); - evt.setValid(msgValid); - cereal::LivePose::Builder livePose = evt.initLivePose(); - this->build_live_pose(livePose); - livePose.setSensorsOK(sensorsOK); - livePose.setInputsOK(inputsOK); -} - -bool Localizer::is_gps_ok() { - return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0; -} - -bool Localizer::critical_services_valid(const std::map &critical_services) { - for (auto &kv : critical_services){ - if (kv.second >= INPUT_INVALID_THRESHOLD){ - return false; - } - } - return true; -} - -bool Localizer::is_timestamp_valid(double current_time) { - double filter_time = this->kf->get_filter_time(); - if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { - LOGE("Observation timestamp is older than the max rewind threshold of the filter"); - return false; - } - return true; -} - -void Localizer::determine_gps_mode(double current_time) { - // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode - // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs - // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is - VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); - if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ - if (this->gps_mode){ - this->gps_mode = false; - this->reset_kalman(current_time); - } else { - this->input_fake_gps_observations(current_time); - } - } -} - -void Localizer::configure_gnss_source(const LocalizerGnssSource &source) { - this->gnss_source = source; - if (source == LocalizerGnssSource::UBLOX) { - this->gps_std_factor = 10.0; - this->gps_variance_factor = 1.0; - this->gps_vertical_variance_factor = 1.0; - this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET; - } else { - this->gps_std_factor = 2.0; - this->gps_variance_factor = 0.0; - this->gps_vertical_variance_factor = 3.0; - this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET; - } -} - -int Localizer::locationd_thread() { - Params params; - LocalizerGnssSource source; - const char* gps_location_socket; - if (params.getBool("UbloxAvailable")) { - source = LocalizerGnssSource::UBLOX; - gps_location_socket = "gpsLocationExternal"; - } else { - source = LocalizerGnssSource::QCOM; - gps_location_socket = "gpsLocation"; - } - - this->configure_gnss_source(source); - const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", - "carState", "accelerometer", "gyroscope"}; - - SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); - PubMaster pm({"livePose"}); - - uint64_t cnt = 0; - bool filterInitialized = false; - const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; - for (std::string service : critical_input_services) { - this->observation_values_invalid.insert({service, 0.0}); - } - - while (!do_exit) { - sm.update(); - if (filterInitialized){ - this->observation_timings_invalid_reset(); - for (const char* service : service_list) { - if (sm.updated(service) && sm.valid(service)){ - const cereal::Event::Reader log = sm[service]; - this->handle_msg(log); - } - } - } else { - filterInitialized = sm.allAliveAndValid(); - } - - const char* trigger_msg = "cameraOdometry"; - if (sm.updated(trigger_msg)) { - bool inputsOK = sm.allValid() && this->are_inputs_ok(); - bool gpsOK = this->is_gps_ok(); - bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); - - // Log time to first fix - if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { - this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); - } - - MessageBuilder pose_msg_builder; - this->build_pose_message(pose_msg_builder, inputsOK, sensorsOK, filterInitialized); - - kj::ArrayPtr pose_bytes = pose_msg_builder.toBytes(); - pm.send("livePose", pose_bytes.begin(), pose_bytes.size()); - - if (cnt % 1200 == 0 && gpsOK) { // once a minute - VectorXd posGeo = this->get_position_geodetic(); - std::string lastGPSPosJSON = util::string_format( - "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); - params.putNonBlocking("LastGPSPosition", lastGPSPosJSON); - } - cnt++; - } - } - return 0; -} - -int main() { - util::set_realtime_priority(5); - - Localizer localizer; - return localizer.locationd_thread(); -} diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h deleted file mode 100644 index f59b7d4bda60f3..00000000000000 --- a/selfdrive/locationd/locationd.h +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/transformations/coordinates.hpp" -#include "common/transformations/orientation.hpp" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -#include "system/sensord/sensors/constants.h" -#define VISION_DECIMATION 2 -#define SENSOR_DECIMATION 10 -#include "selfdrive/locationd/models/live_kf.h" - -#define POSENET_STD_HIST_HALF 20 - -enum LocalizerGnssSource { - UBLOX, QCOM -}; - -class Localizer { -public: - Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); - - int locationd_thread(); - - void reset_kalman(double current_time = NAN); - void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R); - void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P); - void finite_check(double current_time = NAN); - void time_check(double current_time = NAN); - void update_reset_tracker(); - bool is_gps_ok(); - bool critical_services_valid(const std::map &critical_services); - bool is_timestamp_valid(double current_time); - void determine_gps_mode(double current_time); - bool are_inputs_ok(); - void observation_timings_invalid_reset(); - - void build_pose_message( - MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid); - void build_live_pose(cereal::LivePose::Builder& fix); - - Eigen::VectorXd get_position_geodetic(); - Eigen::VectorXd get_state(); - Eigen::VectorXd get_stdev(); - - void handle_msg_bytes(const char *data, const size_t size); - void handle_msg(const cereal::Event::Reader& log); - void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); - void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); - void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); - void handle_car_state(double current_time, const cereal::CarState::Reader& log); - void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); - void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); - - void input_fake_gps_observations(double current_time); - -private: - std::unique_ptr kf; - - Eigen::VectorXd calib; - MatrixXdr device_from_calib; - MatrixXdr calib_from_device; - bool calibrated = false; - - double car_speed = 0.0; - double last_reset_time = NAN; - std::deque posenet_stds; - - std::unique_ptr converter; - - int64_t unix_timestamp_millis = 0; - double reset_tracker = 0.0; - bool gps_mode = false; - double first_valid_log_time = NAN; - double ttff = NAN; - double last_gps_msg = 0; - LocalizerGnssSource gnss_source; - bool observation_timings_invalid = false; - std::map observation_values_invalid; - bool standstill = true; - int32_t orientation_reset_count = 0; - float gps_std_factor; - float gps_variance_factor; - float gps_vertical_variance_factor; - double gps_time_offset; - Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std - - void configure_gnss_source(const LocalizerGnssSource &source); -}; diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc deleted file mode 100644 index fc3bfb72461697..00000000000000 --- a/selfdrive/locationd/models/live_kf.cc +++ /dev/null @@ -1,122 +0,0 @@ -#include "selfdrive/locationd/models/live_kf.h" - -using namespace EKFS; -using namespace Eigen; - -Eigen::Map get_mapvec(const Eigen::VectorXd &vec) { - return Eigen::Map((double*)vec.data(), vec.rows(), vec.cols()); -} - -Eigen::Map get_mapmat(const MatrixXdr &mat) { - return Eigen::Map((double*)mat.data(), mat.rows(), mat.cols()); -} - -std::vector> get_vec_mapvec(const std::vector &vec_vec) { - std::vector> res; - for (const Eigen::VectorXd &vec : vec_vec) { - res.push_back(get_mapvec(vec)); - } - return res; -} - -std::vector> get_vec_mapmat(const std::vector &mat_vec) { - std::vector> res; - for (const MatrixXdr &mat : mat_vec) { - res.push_back(get_mapmat(mat)); - } - return res; -} - -LiveKalman::LiveKalman() { - this->dim_state = live_initial_x.rows(); - this->dim_state_err = live_initial_P_diag.rows(); - - this->initial_x = live_initial_x; - this->initial_P = live_initial_P_diag.asDiagonal(); - this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal(); - this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal(); - this->reset_orientation_P = live_reset_orientation_diag.asDiagonal(); - this->Q = live_Q_diag.asDiagonal(); - for (auto& pair : live_obs_noise_diag) { - this->obs_noise[pair.first] = pair.second.asDiagonal(); - } - - // init filter - this->filter = std::make_shared(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), - get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector(), - std::vector{3}, std::vector(), 0.8); -} - -void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) { - MatrixXdr covs = covs_diag.asDiagonal(); - this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); -} - -void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) { - this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); -} - -void LiveKalman::init_state(const VectorXd &state, double filter_time) { - MatrixXdr covs = this->filter->covs(); - this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); -} - -VectorXd LiveKalman::get_x() { - return this->filter->state(); -} - -MatrixXdr LiveKalman::get_P() { - return this->filter->covs(); -} - -double LiveKalman::get_filter_time() { - return this->filter->get_filter_time(); -} - -std::vector LiveKalman::get_R(int kind, int n) { - std::vector R; - for (int i = 0; i < n; i++) { - R.push_back(this->obs_noise[kind]); - } - return R; -} - -std::optional LiveKalman::predict_and_observe(double t, int kind, const std::vector &meas, std::vector R) { - std::optional r; - if (R.size() == 0) { - R = this->get_R(kind, meas.size()); - } - r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); - return r; -} - -void LiveKalman::predict(double t) { - this->filter->predict(t); -} - -const Eigen::VectorXd &LiveKalman::get_initial_x() { - return this->initial_x; -} - -const MatrixXdr &LiveKalman::get_initial_P() { - return this->initial_P; -} - -const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() { - return this->fake_gps_pos_cov; -} - -const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() { - return this->fake_gps_vel_cov; -} - -const MatrixXdr &LiveKalman::get_reset_orientation_P() { - return this->reset_orientation_P; -} - -MatrixXdr LiveKalman::H(const VectorXd &in) { - assert(in.size() == 6); - Matrix res; - this->filter->get_extra_routine("H")((double*)in.data(), res.data()); - return res; -} diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h deleted file mode 100644 index e4b3e326b3c614..00000000000000 --- a/selfdrive/locationd/models/live_kf.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -#include "generated/live_kf_constants.h" -#include "rednose/helpers/ekf_sym.h" - -#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth) - -using namespace EKFS; - -Eigen::Map get_mapvec(const Eigen::VectorXd &vec); -Eigen::Map get_mapmat(const MatrixXdr &mat); -std::vector> get_vec_mapvec(const std::vector &vec_vec); -std::vector> get_vec_mapmat(const std::vector &mat_vec); - -class LiveKalman { -public: - LiveKalman(); - - void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time); - void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time); - void init_state(const Eigen::VectorXd &state, double filter_time); - - Eigen::VectorXd get_x(); - MatrixXdr get_P(); - double get_filter_time(); - std::vector get_R(int kind, int n); - - std::optional predict_and_observe(double t, int kind, const std::vector &meas, std::vector R = {}); - std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); - std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); - std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind); - void predict(double t); - - const Eigen::VectorXd &get_initial_x(); - const MatrixXdr &get_initial_P(); - const MatrixXdr &get_fake_gps_pos_cov(); - const MatrixXdr &get_fake_gps_vel_cov(); - const MatrixXdr &get_reset_orientation_P(); - - MatrixXdr H(const Eigen::VectorXd &in); - -private: - std::string name = "live"; - - std::shared_ptr filter; - - int dim_state; - int dim_state_err; - - Eigen::VectorXd initial_x; - MatrixXdr initial_P; - MatrixXdr fake_gps_pos_cov; - MatrixXdr fake_gps_vel_cov; - MatrixXdr reset_orientation_P; - MatrixXdr Q; // process noise - std::unordered_map obs_noise; -}; diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py deleted file mode 100755 index 0cc3eca61dd1c8..00000000000000 --- a/selfdrive/locationd/models/live_kf.py +++ /dev/null @@ -1,242 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import os -import numpy as np - -from openpilot.selfdrive.locationd.models.constants import ObservationKind - -import sympy as sp -import inspect -from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate -from rednose.helpers.ekf_sym import gen_code - -EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) - - -def numpy2eigenstring(arr): - assert(len(arr.shape) == 1) - arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '') - return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" - - -class States: - ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters - ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef - ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s - ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s - GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases - ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2 - ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2 - - # Error-state has different slices because it is an ESKF - ECEF_POS_ERR = slice(0, 3) - ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error - ECEF_VELOCITY_ERR = slice(6, 9) - ANGULAR_VELOCITY_ERR = slice(9, 12) - GYRO_BIAS_ERR = slice(12, 15) - ACCELERATION_ERR = slice(15, 18) - ACC_BIAS_ERR = slice(18, 21) - - -class LiveKalman: - name = 'live' - - initial_x = np.array([3.88e6, -3.37e6, 3.76e6, - 0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat - 0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 0, 0, 0]) - - # state covariance - initial_P_diag = np.array([10**2, 10**2, 10**2, - 0.01**2, 0.01**2, 0.01**2, - 10**2, 10**2, 10**2, - 1**2, 1**2, 1**2, - 1**2, 1**2, 1**2, - 100**2, 100**2, 100**2, - 0.01**2, 0.01**2, 0.01**2]) - - # state covariance when resetting midway in a segment - reset_orientation_diag = np.array([1**2, 1**2, 1**2]) - - # fake observation covariance, to ensure the uncertainty estimate of the filter is under control - fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2]) - fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2]) - - # process noise - Q_diag = np.array([0.03**2, 0.03**2, 0.03**2, - 0.001**2, 0.001**2, 0.001**2, - 0.01**2, 0.01**2, 0.01**2, - 0.1**2, 0.1**2, 0.1**2, - (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, - 3**2, 3**2, 3**2, - 0.005**2, 0.005**2, 0.005**2]) - - obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), - ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), - ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), - ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), - ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]), - ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])} - - @staticmethod - def generate_code(generated_dir): - name = LiveKalman.name - dim_state = LiveKalman.initial_x.shape[0] - dim_state_err = LiveKalman.initial_P_diag.shape[0] - - state_sym = sp.MatrixSymbol('state', dim_state, 1) - state = sp.Matrix(state_sym) - x, y, z = state[States.ECEF_POS, :] - q = state[States.ECEF_ORIENTATION, :] - v = state[States.ECEF_VELOCITY, :] - vx, vy, vz = v - omega = state[States.ANGULAR_VELOCITY, :] - vroll, vpitch, vyaw = omega - roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] - acceleration = state[States.ACCELERATION, :] - acc_bias = state[States.ACC_BIAS, :] - - dt = sp.Symbol('dt') - - # calibration and attitude rotation matrices - quat_rot = quat_rotate(*q) - - # Got the quat predict equations from here - # A New Quaternion-Based Kalman Filter for - # Real-Time Attitude Estimation Using the Two-Step - # Geometrically-Intuitive Correction Algorithm - A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], - [vroll, 0, vyaw, -vpitch], - [vpitch, -vyaw, 0, vroll], - [vyaw, vpitch, -vroll, 0]]) - q_dot = A * q - - # Time derivative of the state as a function of state - state_dot = sp.Matrix(np.zeros((dim_state, 1))) - state_dot[States.ECEF_POS, :] = v - state_dot[States.ECEF_ORIENTATION, :] = q_dot - state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration - - # Basic descretization, 1st order intergrator - # Can be pretty bad if dt is big - f_sym = state + dt * state_dot - - state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) - state_err = sp.Matrix(state_err_sym) - quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] - v_err = state_err[States.ECEF_VELOCITY_ERR, :] - omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] - acceleration_err = state_err[States.ACCELERATION_ERR, :] - - # Time derivative of the state error as a function of state error and state - quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) - q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) - state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) - state_err_dot[States.ECEF_POS_ERR, :] = v_err - state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot - state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) - f_err_sym = state_err + dt * state_err_dot - - # Observation matrix modifier - H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) - H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) - H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:] - H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop) - - # these error functions are defined so that say there - # is a nominal x and true x: - # true x = err_function(nominal x, delta x) - # delta x = inv_err_function(nominal x, true x) - nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) - true_x = sp.MatrixSymbol('true_x', dim_state, 1) - delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) - - err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) - delta_quat = sp.Matrix(np.ones(4)) - delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :]) - err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :]) - err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat - err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :]) - - inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) - inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0]) - delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0] - inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:]) - inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0]) - - eskf_params = [[err_function_sym, nom_x, delta_x], - [inv_err_function_sym, nom_x, true_x], - H_mod_sym, f_err_sym, state_err_sym] - # - # Observation functions - # - h_gyro_sym = sp.Matrix([ - vroll + roll_bias, - vpitch + pitch_bias, - vyaw + yaw_bias]) - - pos = sp.Matrix([x, y, z]) - gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) - h_acc_sym = (gravity + acceleration + acc_bias) - h_acc_stationary_sym = acceleration - h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) - h_pos_sym = sp.Matrix([x, y, z]) - h_vel_sym = sp.Matrix([vx, vy, vz]) - h_orientation_sym = q - h_relative_motion = sp.Matrix(quat_rot.T * v) - - obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], - [h_phone_rot_sym, ObservationKind.NO_ROT, None], - [h_acc_sym, ObservationKind.PHONE_ACCEL, None], - [h_pos_sym, ObservationKind.ECEF_POS, None], - [h_vel_sym, ObservationKind.ECEF_VEL, None], - [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None], - [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], - [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], - [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] - - # this returns a sympy routine for the jacobian of the observation function of the local vel - in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz - h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) - extra_routines = [('H', h.jacobian(in_vec), [in_vec])] - - gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) - - # write constants to extra header file for use in cpp - live_kf_header = "#pragma once\n\n" - live_kf_header += "#include \n" - live_kf_header += "#include \n\n" - for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)): - assert(slc.step is None) # unsupported - live_kf_header += f'#define STATE_{state}_START {slc.start}\n' - live_kf_header += f'#define STATE_{state}_END {slc.stop}\n' - live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n' - live_kf_header += "\n" - - for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)): - live_kf_header += f'#define OBSERVATION_{kind} {val}\n' - live_kf_header += "\n" - - live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n" - live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n" - live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n" - live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n" - live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n" - live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n" - live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n" - for kind, noise in LiveKalman.obs_noise_diag.items(): - live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n" - live_kf_header += "};\n\n" - - open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) - - -if __name__ == "__main__": - generated_dir = sys.argv[2] - LiveKalman.generate_code(generated_dir)