Compare commits

..

226 Commits

Author SHA1 Message Date
royjr d820833637 distless 2026-11-02 17:11:48 -05:00
royjr f9d956108d park 2026-11-02 16:31:32 -05:00
royjr 1ad66c7e40 summary 2026-11-01 06:27:18 -05:00
royjr 561cf81abf Update opendbc_repo 2026-10-31 22:14:30 -04:00
royjr 79dc3d155e Update opendbc_repo 2026-10-30 10:37:34 -04:00
royjr 98f47fbdbb Merge branch 'master' into ccnc-port 2026-10-29 11:35:40 -04:00
royjr 5a950c6524 Merge branch 'master' into ccnc-port 2026-10-27 14:13:44 -04:00
royjr ae2df123c5 Update opendbc_repo 2026-10-27 14:13:18 -04:00
royjr fddd0a35f4 Merge branch 'master' into ccnc-port 2026-10-25 23:39:51 -04:00
royjr 49bc83f623 Merge branch 'master' into ccnc-port 2026-10-23 01:07:54 -04:00
royjr ec59771282 Update opendbc_repo 2026-10-23 01:07:50 -04:00
royjr a06f1bc571 Merge branch 'master' into ccnc-port 2026-10-22 16:28:38 -04:00
royjr db02972082 Merge branch 'master' into ccnc-port 2026-10-19 20:24:20 -04:00
royjr 6c5711081b Update opendbc_repo 2026-10-19 20:24:12 -04:00
royjr f5ea257318 Merge branch 'master' into ccnc-port 2026-10-16 12:56:47 -04:00
royjr 92a39eb58c Merge branch 'master' into ccnc-port 2026-10-16 12:40:49 -04:00
royjr 107a14650e Merge branch 'master' into ccnc-port 2026-10-07 13:42:56 -04:00
royjr 9dd71b14a7 Update opendbc_repo 2026-10-07 13:42:44 -04:00
royjr 45f7fcdd05 Update opendbc_repo 2026-09-28 00:28:40 -04:00
royjr dfd06dbf69 Update opendbc_repo 2026-09-23 21:47:39 -04:00
royjr 110f453d5f Update opendbc_repo 2026-09-17 00:04:11 -04:00
royjr 320b9e8783 Merge branch 'master' into ccnc-port 2026-09-16 13:01:07 -04:00
royjr 12f2c30c14 Merge branch 'master' into ccnc-port 2026-09-07 00:44:44 -04:00
royjr 0999b0cbe9 Merge branch 'master' into ccnc-port 2026-09-02 15:16:21 -04:00
royjr ed1121e61c Update opendbc_repo 2026-09-02 15:16:02 -04:00
royjr 50d7f75bfc Merge branch 'master' into ccnc-port 2026-08-30 16:21:17 -04:00
royjr 15c7f52e40 Merge branch 'master' into ccnc-port 2026-08-28 20:28:33 -04:00
royjr 8dfe04a318 Merge branch 'master' into ccnc-port 2026-08-24 14:19:36 -04:00
royjr 9648ac2f04 Merge branch 'master' into ccnc-port 2026-08-23 21:48:17 -04:00
royjr a03a11e333 Merge branch 'master' into ccnc-port 2026-08-23 19:36:45 -04:00
royjr 53b1070b09 Merge branch 'master' into ccnc-port 2026-08-23 11:05:40 -04:00
royjr 68bf6d8162 Merge branch 'master' into ccnc-port 2026-08-22 13:19:05 -04:00
royjr 8b472291d0 Merge branch 'master' into ccnc-port 2026-08-21 17:12:13 -04:00
royjr 797c8d4293 Update opendbc_repo 2026-08-21 17:09:31 -04:00
royjr 7568528507 Merge branch 'master' into ccnc-port 2026-08-19 12:53:59 -04:00
royjr 6665c3acf6 Merge branch 'master' into ccnc-port 2026-08-17 10:14:33 -04:00
royjr 3b13eb4d0d Update opendbc_repo 2026-08-17 10:14:19 -04:00
royjr f7eb4c2561 Update opendbc_repo 2026-08-15 01:39:27 -04:00
royjr be9255358c Update opendbc_repo 2026-08-15 01:35:33 -04:00
royjr f48ccb5d7a Merge branch 'master' into ccnc-port 2026-08-14 20:42:45 -04:00
royjr 99559d6749 Update opendbc_repo 2026-08-14 20:42:36 -04:00
royjr 4b6f0ffb46 Merge branch 'master' into ccnc-port 2026-08-11 11:56:46 -04:00
royjr 55415382ce Update opendbc_repo 2026-08-11 11:56:38 -04:00
royjr 9f30901eb6 Merge branch 'master' into ccnc-port 2026-08-10 16:33:50 -04:00
royjr d2045d24fb Update opendbc_repo 2026-08-10 16:33:37 -04:00
royjr 6488a11e4d Merge branch 'master' into ccnc-port 2026-08-02 14:27:34 -04:00
royjr 7b22b65313 Merge branch 'master' into ccnc-port 2026-07-27 15:29:04 -04:00
royjr b59a50101e Update opendbc_repo 2026-07-27 15:28:51 -04:00
royjr 3e30962bd3 Merge branch 'master' into ccnc-port 2026-07-25 03:04:23 -04:00
royjr 3274043063 Update opendbc_repo 2026-07-25 03:04:12 -04:00
royjr c624550d2e Merge branch 'master' into ccnc-port 2026-07-20 15:52:41 -04:00
royjr 6759038671 Merge branch 'master' into ccnc-port 2026-07-20 09:18:18 -04:00
royjr aa636e75c8 Merge branch 'master' into ccnc-port 2026-07-16 00:23:49 -04:00
royjr 5ff3c3bd8d Update opendbc_repo 2026-07-16 00:23:43 -04:00
royjr 1e11b52290 Update opendbc_repo 2026-07-14 00:40:02 -04:00
royjr 76e0204025 Update opendbc_repo 2026-07-14 00:37:13 -04:00
royjr cc88f2cbd6 Update opendbc_repo 2026-07-10 22:00:25 -04:00
royjr ee0ac199a4 Update opendbc_repo 2026-07-03 08:08:29 -04:00
royjr d5ed828eaa Update opendbc_repo 2026-07-01 19:30:06 -04:00
royjr 9fca585f2a Merge branch 'master' into ccnc-port 2026-06-30 22:14:33 -04:00
royjr bb1259303e Update opendbc_repo 2026-06-30 22:14:23 -04:00
royjr e135051ca8 Merge branch 'master' into ccnc-port 2026-06-13 22:38:22 -04:00
royjr 12bef55d8a Update opendbc_repo 2026-06-13 22:12:00 -04:00
royjr 2f7a45e6c8 Merge branch 'master' into ccnc-port 2026-06-08 22:01:57 -04:00
royjr 936ebfc12b Update opendbc_repo 2026-06-08 22:01:44 -04:00
royjr aa0c9dc0eb Merge branch 'master' into ccnc-port 2026-06-04 09:52:35 -04:00
royjr 7476a866e7 Update opendbc_repo 2026-06-04 09:52:06 -04:00
royjr 610d857e33 Merge branch 'master' into ccnc-port 2026-05-28 08:30:40 -04:00
royjr d2f47407d0 Update opendbc_repo 2026-05-12 21:03:18 -04:00
royjr db75ec76ea Merge branch 'master' into ccnc-port 2026-05-12 20:44:57 -04:00
royjr 24066465d7 Update opendbc_repo 2026-05-12 20:44:48 -04:00
royjr a7abbd6e25 Update opendbc_repo 2026-04-20 18:22:12 -04:00
royjr 878982447c Merge branch 'master' into ccnc-port 2026-04-19 12:06:46 -04:00
royjr 576527a36b Merge branch 'master' into ccnc-port 2026-04-17 05:42:35 -04:00
royjr ef8c35da24 Update opendbc_repo 2026-04-17 05:41:21 -04:00
royjr 85688b1040 Merge branch 'master' into ccnc-port 2026-04-16 19:48:39 -04:00
royjr 0fb2199130 Update opendbc_repo 2026-04-16 19:47:57 -04:00
royjr d48d756c1d Update opendbc_repo 2026-04-08 00:56:54 -04:00
royjr 2ed298a0c9 Merge branch 'master' into ccnc-port 2026-04-08 00:51:13 -04:00
royjr d68f038949 Update opendbc_repo 2026-04-08 00:51:11 -04:00
royjr 7231571e57 Merge branch 'master' into ccnc-port 2026-04-03 23:34:00 -04:00
royjr b37f1419d3 Update opendbc_repo 2026-04-03 23:33:19 -04:00
royjr cd85a66790 Merge branch 'master' into ccnc-port 2026-03-26 00:53:07 -04:00
royjr 305ea87daf Update opendbc_repo 2026-03-26 00:52:44 -04:00
royjr 4bbfc793e0 Merge branch 'master' into ccnc-port 2026-03-15 15:44:59 -04:00
royjr d5d983676e Update opendbc_repo 2026-03-13 16:41:05 -04:00
royjr de8a96a398 Merge branch 'master' into ccnc-port 2026-03-13 16:41:00 -04:00
royjr 0cbf45f699 Merge branch 'master' into ccnc-port 2026-03-11 23:28:59 -04:00
royjr 0d68a3a2ab Merge branch 'master' into ccnc-port 2026-03-09 19:58:32 -04:00
royjr 9e85a85059 Update opendbc_repo 2026-03-09 19:58:18 -04:00
royjr 0373c327c0 Update opendbc_repo 2026-03-02 10:10:38 -05:00
royjr efe9e5c200 Update opendbc_repo 2026-03-02 02:15:05 -05:00
royjr 8a249a45dc Update opendbc_repo 2026-03-02 02:06:16 -05:00
royjr bdbefe67f6 Update opendbc_repo 2026-03-02 01:40:31 -05:00
royjr 675bb166ad Merge branch 'master' into ccnc-port 2026-03-01 17:18:11 -05:00
royjr 1b717a7e88 Merge branch 'master' into ccnc-port 2026-03-01 13:23:30 -05:00
royjr 86f55a8ba9 Update opendbc_repo 2026-03-01 13:23:24 -05:00
royjr 629392d2f7 Update opendbc_repo 2026-02-27 16:31:59 -05:00
royjr bc414bdc8b Update opendbc_repo 2026-02-26 23:53:26 -05:00
royjr 7ca5649f2c Merge branch 'master' into ccnc-port 2026-02-26 23:52:46 -05:00
royjr 641ee8fa87 Update opendbc_repo 2026-02-26 23:52:27 -05:00
royjr 56c276158c Merge branch 'master' into ccnc-port 2026-02-24 14:12:12 -05:00
royjr c65308a8bd Update opendbc_repo 2026-02-24 14:12:01 -05:00
royjr 994e526460 Merge branch 'master' into ccnc-port 2026-02-18 12:06:05 -05:00
royjr 1defae36b7 Update opendbc_repo 2026-02-18 12:05:53 -05:00
royjr 8f029fd0ef Merge branch 'master' into ccnc-port 2026-02-13 23:01:41 -05:00
royjr ddb46284dc Update opendbc_repo 2026-02-13 23:01:16 -05:00
royjr 9effc754d9 Merge branch 'master' into ccnc-port 2026-02-06 01:16:06 -05:00
royjr e49ffc2a2d Update opendbc_repo 2026-02-06 01:15:59 -05:00
royjr 2cacd0b3e5 Merge branch 'master' into ccnc-port 2026-01-24 12:50:18 -05:00
royjr c4b8859dff Update opendbc_repo 2026-01-24 12:50:11 -05:00
royjr 8fb0953205 Merge branch 'master' into ccnc-port 2026-01-11 22:17:38 -05:00
royjr 63d1c8835f Merge branch 'master' into ccnc-port 2026-01-10 13:03:48 -05:00
royjr 17a185606d Merge branch 'master' into ccnc-port 2026-01-09 16:40:32 -05:00
royjr da10131392 Merge branch 'master' into ccnc-port 2025-12-28 17:18:51 -05:00
royjr 7107c2ba14 Merge branch 'master' into ccnc-port 2025-12-23 12:13:48 -05:00
royjr 95b6e877ac Update opendbc_repo 2025-12-23 12:13:29 -05:00
royjr eb02c6570e Update opendbc_repo 2025-12-21 15:43:04 -05:00
royjr 1be8ae31c4 Merge branch 'master' into ccnc-port 2025-12-19 01:04:42 -05:00
royjr 04dcd38856 Update opendbc_repo 2025-12-19 01:04:30 -05:00
royjr 22ccf0d72f Merge branch 'master' into ccnc-port 2025-12-15 17:02:50 -05:00
royjr 3c969bb627 Merge branch 'master' into ccnc-port 2025-12-13 23:22:22 -05:00
royjr 20f8011feb Update opendbc_repo 2025-12-13 23:22:11 -05:00
royjr 9cf17e74a1 Merge branch 'master' into ccnc-port 2025-12-12 23:19:56 -05:00
royjr 2c4efdf557 Merge branch 'master' into ccnc-port 2025-12-07 13:29:48 -05:00
royjr 4cd3d3c16c Merge branch 'master' into ccnc-port 2025-12-02 12:56:21 -05:00
royjr 637f3ae9c8 Merge branch 'master' into ccnc-port 2025-12-01 14:41:03 -05:00
royjr 464ee80f71 Merge branch 'master' into ccnc-port 2025-11-26 00:27:53 -05:00
royjr 2743a04613 Merge branch 'master' into ccnc-port 2025-11-24 18:44:13 -05:00
royjr 7f9978d001 Merge branch 'master' into ccnc-port 2025-11-22 00:21:15 -05:00
royjr 4b83961c67 Merge branch 'master' into ccnc-port 2025-11-21 16:23:22 -05:00
royjr c00eaf428a Update opendbc_repo 2025-11-21 16:23:01 -05:00
royjr 0a9993e8d4 Merge branch 'master' into ccnc-port 2025-11-19 16:49:59 -05:00
royjr 0af214a985 Update opendbc_repo 2025-11-19 16:49:51 -05:00
royjr af43385e3a Merge branch 'master' into ccnc-port 2025-11-11 10:19:51 -05:00
royjr 0ab2b8c590 Update opendbc_repo 2025-11-07 19:59:50 -05:00
royjr 67ab18a0de Merge branch 'master' into ccnc-port 2025-11-07 19:23:51 -05:00
royjr e87dc15b30 Update opendbc_repo 2025-11-07 19:23:37 -05:00
royjr 192d08516c Merge branch 'master' into ccnc-port 2025-11-02 19:23:00 -05:00
royjr 3cf001c59c Update opendbc_repo 2025-11-02 19:22:49 -05:00
royjr f2ccd021da Merge branch 'master' into ccnc-port 2025-11-02 14:07:54 -05:00
royjr c9fc900f64 Update opendbc_repo 2025-11-02 14:07:44 -05:00
royjr 3c37c5ce5d Update opendbc_repo 2025-10-30 11:28:49 -04:00
royjr 7c45889e4e Merge branch 'master' into ccnc-port 2025-10-30 11:27:50 -04:00
royjr 2aabb7aee8 Merge branch 'master' into ccnc-port 2025-10-24 14:16:09 -04:00
royjr 3859e9962f Update opendbc_repo 2025-10-24 14:15:51 -04:00
royjr 810efbab72 Merge branch 'master' into ccnc-port 2025-10-18 07:33:11 -04:00
royjr ec27bec326 Update opendbc_repo 2025-10-18 07:32:33 -04:00
royjr 250d553157 Merge branch 'master' into ccnc-port 2025-10-14 21:57:32 -04:00
royjr cea54a0ca8 Update opendbc_repo 2025-10-14 21:57:26 -04:00
royjr 8e72d783bd Update opendbc_repo 2025-10-13 22:41:21 -04:00
royjr 1b0dc103dc Merge branch 'master' into ccnc-port 2025-10-11 23:51:46 -04:00
royjr 6c364d292b Update opendbc_repo 2025-10-11 23:51:31 -04:00
royjr bcdec2ce84 Merge branch 'master' into ccnc-port 2025-10-10 17:29:05 -04:00
royjr 3deaeb3759 Merge branch 'master' into ccnc-port 2025-10-10 15:02:32 -04:00
royjr c669f0984a Update opendbc_repo 2025-10-10 15:02:17 -04:00
royjr 46dd946740 Merge branch 'master' into ccnc-port 2025-10-07 01:36:06 -04:00
royjr 9da4b3653e Update opendbc_repo 2025-10-07 01:35:58 -04:00
royjr 4e21ae7c50 Update opendbc_repo 2025-10-05 06:21:56 -04:00
royjr bb91e92237 Update opendbc_repo 2025-10-05 06:01:15 -04:00
royjr 14b4c4f85b Update opendbc_repo 2025-10-02 09:20:32 -04:00
royjr 0660b542c3 Merge branch 'master' into ccnc-port 2025-10-01 16:01:32 -04:00
royjr 2b893b90c9 Update opendbc_repo 2025-10-01 16:01:26 -04:00
royjr f5139178ed Merge branch 'master' into ccnc-port 2025-09-30 14:39:57 -04:00
royjr fb43b755f2 Update opendbc_repo 2025-09-30 14:39:50 -04:00
royjr 07f5b967d8 Merge branch 'master' into ccnc-port 2025-09-24 21:30:29 -04:00
royjr ea19c7d3bb Update opendbc_repo 2025-09-24 21:30:17 -04:00
royjr e461842cbb Merge branch 'master' into ccnc-port 2025-09-23 05:55:02 -04:00
royjr a73c9659d5 Update opendbc_repo 2025-09-23 05:54:50 -04:00
royjr cb796fbc76 Merge branch 'master' into ccnc-port 2025-09-18 20:10:43 -04:00
royjr 6bf75fc557 Update opendbc_repo 2025-09-18 20:10:37 -04:00
royjr 9a1fc28819 Merge branch 'master' into ccnc-port 2025-09-18 13:54:36 -04:00
royjr 0741d05e92 Update opendbc_repo 2025-09-18 13:54:23 -04:00
royjr 1ad008107d Merge branch 'master' into ccnc-port 2025-09-15 01:40:27 -04:00
royjr feebd9df93 Reapply "UI: Developer UI (#1233)"
This reverts commit 15e5d2efb9.
2025-09-15 01:40:21 -04:00
royjr c2e5ced3e5 Update opendbc_repo 2025-09-15 01:39:51 -04:00
royjr 15e5d2efb9 Revert "UI: Developer UI (#1233)"
This reverts commit 1bb4ca2547.
2025-09-12 02:10:29 -04:00
royjr a3929d0b54 Merge branch 'master' into ccnc-port 2025-09-12 01:40:29 -04:00
royjr 794f8f9991 Update opendbc_repo 2025-09-08 09:27:31 -04:00
royjr 68fa5e3f21 Merge branch 'master' into ccnc-port 2025-09-07 13:13:37 -04:00
royjr 86c6cc1f48 Merge branch 'master' into ccnc-port 2025-09-03 22:22:12 -04:00
royjr eb7ffbf093 Update opendbc_repo 2025-09-03 10:14:58 -04:00
royjr 3919095752 Update opendbc_repo 2025-09-03 10:05:35 -04:00
royjr 74d63be1c3 Merge branch 'master' into ccnc-port 2025-09-03 09:50:55 -04:00
royjr 8894486a1a Update opendbc_repo 2025-09-03 09:50:49 -04:00
royjr 810599315d Merge branch 'master' into ccnc-port 2025-08-31 16:53:30 -04:00
royjr 6f3ab810c8 Update opendbc_repo 2025-08-31 16:53:24 -04:00
royjr 230f78b8d3 Merge branch 'master' into ccnc-port 2025-08-26 12:14:46 -04:00
royjr f1affec088 Update opendbc_repo 2025-08-26 12:14:22 -04:00
royjr 97d8ef242c Merge branch 'master' into ccnc-port 2025-08-24 15:12:57 -04:00
royjr a63fff9b45 Update opendbc_repo 2025-08-24 15:12:46 -04:00
royjr cb3893daaa Merge branch 'master' into ccnc-port 2025-08-23 10:33:24 -04:00
royjr 29f60df74b Merge branch 'master' into ccnc-port 2025-08-22 11:18:27 -04:00
royjr c6c072e1f4 Update opendbc_repo 2025-08-22 11:18:18 -04:00
royjr d101cbb83e Update opendbc_repo 2025-08-13 16:00:04 -04:00
royjr 1536d59633 Update opendbc_repo 2025-08-13 15:28:37 -04:00
royjr dc99b865ae Merge branch 'master' into ccnc-port 2025-08-13 12:31:14 -04:00
royjr e59bc027ff Merge branch 'master' into ccnc-port 2025-08-13 11:58:05 -04:00
royjr cf7e5efaca Update opendbc_repo 2025-08-13 11:57:57 -04:00
royjr 4b44f2eb31 Merge branch 'master' into ccnc-port 2025-08-10 09:18:24 -04:00
royjr 107d2ab400 Update opendbc_repo 2025-08-10 09:18:15 -04:00
royjr 5432d9062c Merge branch 'master' into ccnc-port 2025-08-04 11:52:29 -04:00
royjr f533f6c843 Merge branch 'master' into ccnc-port 2025-08-02 06:53:54 -04:00
royjr 58e9ac763c Update opendbc_repo 2025-08-02 06:53:43 -04:00
royjr cb50d54169 Merge branch 'master-new' into ccnc-port 2025-07-24 20:23:46 -04:00
royjr bd5de4ed0a Merge branch 'master-new' into ccnc-port 2025-07-20 23:49:36 -04:00
royjr 0d4073fadb Merge branch 'master-new' into ccnc-port 2025-07-19 23:18:26 -04:00
royjr ebc70dcb52 Merge branch 'master-new' into ccnc-port 2025-07-19 14:37:17 -04:00
royjr 4d0426999e Update opendbc_repo 2025-07-19 14:36:59 -04:00
royjr 286da42573 Merge branch 'master-new' into ccnc-port 2025-07-16 23:48:23 -04:00
royjr 8a836710a9 Update opendbc_repo 2025-07-16 23:48:06 -04:00
royjr 5d515bcf33 Merge branch 'master-new' into ccnc-port 2025-07-07 05:35:55 -04:00
royjr 1c7f6d5133 Update opendbc_repo 2025-07-03 21:24:01 -04:00
royjr 05d57c7aeb Update opendbc_repo 2025-07-01 18:30:32 -04:00
royjr e4b0eaf352 Update opendbc_repo 2025-06-28 19:30:28 -04:00
royjr a710276472 Merge branch 'master-new' into ccnc-port 2025-06-28 19:22:59 -04:00
royjr af086db671 Merge branch 'master-new' into ccnc-port 2025-06-28 12:13:26 -04:00
royjr 0d9eb0e25e Update opendbc_repo submodule to latest commit
Advanced the opendbc_repo submodule to commit d309f7ec96e37267c94d12fc4bfe2672ad505b06. This pulls in the latest changes from the opendbc repository.
2025-06-26 14:03:56 -04:00
royjr 0616caed6d Merge branch 'master-new' into ccnc-port 2025-06-25 19:33:53 -04:00
royjr 095337b3c1 Update opendbc_repo 2025-06-25 19:33:38 -04:00
royjr 1edec2d22c Update opendbc_repo 2025-06-14 16:14:02 -04:00
royjr affabb9ee0 Update opendbc_repo 2025-06-14 15:55:07 -04:00
royjr dc27e8711c Update opendbc_repo 2025-06-14 14:54:32 -04:00
royjr cf7329a264 Merge branch 'master-new' into ccnc-port 2025-06-11 21:36:06 -04:00
royjr 5ee5ecd820 Merge branch 'master-new' into ccnc-port 2025-06-08 23:25:13 -04:00
royjr b064f730dd Update opendbc_repo 2025-06-08 17:54:43 -04:00
66 changed files with 2373 additions and 1016 deletions
+1 -1
View File
@@ -42,7 +42,7 @@ jobs:
WARP_PKL="openpilot/sunnypilot/modeld_v2/models/driving_warp_${res}_tinygrad.pkl"
echo "Compiling $WARP_PKL on QCOM"
taskset -c 7 env DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 python3 openpilot/sunnypilot/modeld_v2/compile_warp.py \
--frame ${FRAME_PARAM} --warp-to ${MODEL_SIZE} --split-frames --output "${WARP_PKL}"
--frame ${FRAME_PARAM} --warp-to ${MODEL_SIZE} --output "${WARP_PKL}"
BIG_WARP_PKL="openpilot/sunnypilot/modeld_v2/models/big_driving_warp_${res}_tinygrad.pkl"
echo "Compiling $BIG_WARP_PKL on AMD"
@@ -168,7 +168,7 @@ jobs:
echo "Building locationd..."
# -j1: parallel rednose generators OOM the device
scons -j1 cache_dir="$SCONS_CACHE" --minimal \
openpilot/selfdrive/locationd
openpilot/selfdrive/locationd openpilot/sunnypilot/selfdrive/locationd
echo "Building rest of sunnypilot"
sudo rm -rf /tmp/* || true
mkdir -p "$BUILD_DIR/tmp"
-17
View File
@@ -46,23 +46,6 @@ jobs:
CHECK_OOB: 0
GMMU: 0
run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py
- name: Resolve commaai's tinygrad pin, which their precompiled chestnut model is built and converted with
id: upstream-tg
env:
GH_TOKEN: ${{ github.token }}
run: |
read -r sha url < <(gh api repos/commaai/openpilot/contents/tinygrad_repo --jq '.sha + " " + .submodule_git_url')
echo "commaai/openpilot master pins $url@$sha, ours is $(git rev-parse HEAD:tinygrad_repo)"
dir="$RUNNER_TEMP/upstream_tinygrad"
git init -q "$dir" && git -C "$dir" fetch -q --depth 1 "$url" "$sha" && git -C "$dir" checkout -q FETCH_HEAD
echo "path=$dir" >> "$GITHUB_OUTPUT"
- name: Convert a pickled USB+AMD jit under commaai's tinygrad, as the chestnut model build does
env:
DEV: MOCKUSB+AMD
CHECK_OOB: 0
GMMU: 0
PYTHONPATH: ${{ steps.upstream-tg.outputs.path }}:${{ github.workspace }}
run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py
- name: Where tinygrad moved, for the helper or failure point named above
if: failure() && github.event_name == 'pull_request'
run: |
+2
View File
@@ -293,6 +293,8 @@ SConscript([
'openpilot/selfdrive/ui/SConscript',
])
SConscript(['openpilot/sunnypilot/SConscript'])
# Build desktop-only tools
if GetOption('extras') and arch != "comma_arm64":
SConscript([
+2 -9
View File
@@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
# 358 Supported Cars
# 351 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br>&nbsp;|Video|Setup Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -139,12 +139,10 @@ A supported vehicle is one that just works when you install a comma device. All
|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Ioniq Plug-in Hybrid 2020-22">Buy Here</a></sub></details>|||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona 2020">Buy Here</a></sub></details>|||
|Hyundai|Kona 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona 2022-23">Buy Here</a></sub></details>|||
|Hyundai|Kona (without HDA II) 2024-25|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona (without HDA II) 2024-25">Buy Here</a></sub></details>|||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric 2018-21">Buy Here</a></sub></details>|||
|Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric 2022-23">Buy Here</a></sub></details>|||
|Hyundai|Kona Electric (with HDA II, Korea only) 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric (with HDA II, Korea only) 2023">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=U2fOCmcQ8hw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai I connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Hybrid 2020">Buy Here</a></sub></details>|||
|Hyundai|Kona Hybrid (without HDA II) 2024-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Hybrid (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|Hyundai|Nexo 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Nexo 2021">Buy Here</a></sub></details>|||
|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Palisade 2020-22">Buy Here</a></sub></details>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Hyundai|Santa Cruz 2022-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Santa Cruz 2022-24">Buy Here</a></sub></details>|||
@@ -154,23 +152,19 @@ A supported vehicle is one that just works when you install a comma device. All
|Hyundai|Santa Fe Plug-in Hybrid 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Santa Fe Plug-in Hybrid 2022-23">Buy Here</a></sub></details>|||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata 2018-19">Buy Here</a></sub></details>|||
|Hyundai|Sonata 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata 2020-23">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Hyundai|Sonata (without HDA II) 2024-26|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|Hyundai|Sonata Hybrid 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata Hybrid 2020-23">Buy Here</a></sub></details>|||
|Hyundai|Sonata Hybrid (without HDA II) 2024-26|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata Hybrid (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|Hyundai|Staria 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Staria 2023">Buy Here</a></sub></details>|||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson 2021">Buy Here</a></sub></details>|||
|Hyundai|Tucson 2022|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson 2022">Buy Here</a></sub></details>|||
|Hyundai|Tucson 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson 2023-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Diesel 2019">Buy Here</a></sub></details>|||
|Hyundai|Tucson Hybrid 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid 2022-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Hybrid (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|Hyundai|Tucson Plug-in Hybrid 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2023-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2024">Buy Here</a></sub></details>|||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Veloster 2019-20">Buy Here</a></sub></details>|||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2019-21">Buy Here</a></sub></details>|||
|Kia|Carnival 2022-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Carnival 2022-24">Buy Here</a></sub></details>|||
|Kia|Carnival (China only) 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Carnival (China only) 2023">Buy Here</a></sub></details>|||
|Kia|Carnival Hybrid (with HDA II) 2025-26|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Carnival Hybrid (with HDA II) 2025-26">Buy Here</a></sub></details>|||
|Kia|Ceed 2019-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Ceed 2019-21">Buy Here</a></sub></details>|||
|Kia|Ceed Plug-in Hybrid 2021|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Ceed Plug-in Hybrid 2021">Buy Here</a></sub></details>|||
|Kia|EV6 (Southeast Asia only) 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia EV6 (Southeast Asia only) 2022-24">Buy Here</a></sub></details>|||
@@ -196,7 +190,6 @@ A supported vehicle is one that just works when you install a comma device. All
|Kia|Niro Plug-in Hybrid 2020|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plug-in Hybrid 2020">Buy Here</a></sub></details>|||
|Kia|Niro Plug-in Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plug-in Hybrid 2021">Buy Here</a></sub></details>|||
|Kia|Niro Plug-in Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plug-in Hybrid 2022">Buy Here</a></sub></details>|||
|Kia|Niro Plus EV 2024|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plus EV 2024">Buy Here</a></sub></details>|||
|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Optima 2017">Buy Here</a></sub></details>|||
|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Optima 2019-20">Buy Here</a></sub></details>|||
|Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Optima Hybrid 2019">Buy Here</a></sub></details>|||
+1 -10
View File
@@ -483,16 +483,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
}
}
struct GpsLocationSP @0xcb9fd56c7057593a {
latitude @0 :Float64;
longitude @1 :Float64;
speed @2 :Float32;
bearingDeg @3 :Float32;
horizontalAccuracy @4 :Float32; # m
bearingAccuracyDeg @5 :Float32;
hasFix @6 :Bool;
gpsOK @7 :Bool; # a raw fix arrived in the last 2 s
fused @8 :Bool; # false while passing the raw fix through
struct CustomReserved10 @0xcb9fd56c7057593a {
}
struct CustomReserved11 @0xc2243c65e0340384 {
+2 -2
View File
@@ -2648,7 +2648,7 @@ struct Event {
carStateSP @114 :Custom.CarStateSP;
liveMapDataSP @115 :Custom.LiveMapDataSP;
modelDataV2SP @116 :Custom.ModelDataV2SP;
gpsLocationSP @136 :Custom.GpsLocationSP;
customReserved10 @136 :Custom.CustomReserved10;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
@@ -2701,7 +2701,7 @@ struct Event {
lateralPlanDEPRECATED @64 :LateralPlan;
navModelDEPRECATED @104 :Deprecated.NavModelData;
uiPlanDEPRECATED @106 :UiPlan;
liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
liveLocationKalman @72 :LiveLocationKalman;
liveTracksDEPRECATED @16 :List(Deprecated.LiveTracksDEPRECATED);
onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED);
gyroscope2DEPRECATED @100 :SensorEventData;
+1 -1
View File
@@ -95,7 +95,7 @@ _services: dict[str, tuple] = {
"carStateSP": (True, 100., 10),
"liveMapDataSP": (True, 1., 1),
"modelDataV2SP": (True, 20., 10, QueueSize.BIG),
"gpsLocationSP": (True, 20., 20),
"liveLocationKalman": (True, 20.),
# debug
"uiDebug": (True, 0., 1),
+3 -2
View File
@@ -149,7 +149,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"Version", {PERSISTENT, STRING}},
// --- sunnypilot params --- //
{"AccelBoostSP", {PERSISTENT | BACKUP, BOOL, "1"}},
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
@@ -172,13 +171,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
{"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
{"HideVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
{"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
{"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
{"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"LastGPSPositionFused", {PERSISTENT, STRING}},
{"LastGPSPositionLLK", {PERSISTENT, STRING}},
{"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
{"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
@@ -199,6 +199,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
{"TripSummary", {PERSISTENT | BACKUP, BOOL, "1"}},
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
// MADS params
+2
View File
@@ -10,6 +10,7 @@ from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView
from openpilot.selfdrive.ui.ui_state import device, ui_state
from openpilot.selfdrive.ui.layouts.onboarding import OnboardingWindow
from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
from openpilot.selfdrive.ui.sunnypilot.layouts.trip_summary import TripSummary
if gui_app.sunnypilot_ui():
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.settings import SettingsLayoutSP as SettingsLayout
@@ -31,6 +32,7 @@ class MainLayout(Widget):
self._sidebar = Sidebar()
self._current_mode = MainState.HOME
self._prev_onroad = False
self._trip_summary = TripSummary()
# Initialize layouts
self._home_layout = HomeLayout()
@@ -10,6 +10,7 @@ from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
from openpilot.system.ui.widgets import Widget
from openpilot.system.ui.widgets.scroller import Scroller
from openpilot.system.ui.lib.application import gui_app
from openpilot.selfdrive.ui.sunnypilot.layouts.trip_summary import TripSummary
if gui_app.sunnypilot_ui():
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
@@ -57,6 +58,7 @@ class MiciMainLayout(Scroller):
self._setup_callbacks()
gui_app.add_nav_stack_tick(self._handle_transitions)
self._trip_summary = TripSummary()
gui_app.push_widget(self)
# Start onboarding if terms or training not completed, make sure to push after self
@@ -57,12 +57,6 @@ class CruiseLayout(Widget):
description=tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
param="SmartCruiseControlMap")
self.accel_boost_toggle = toggle_item_sp(
title=tr("Acceleration Boost"),
description=tr("Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. " +
"Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first."),
param="AccelBoostSP")
self.custom_acc_toggle = toggle_item_sp(
title=tr("Custom ACC Speed Increments"),
description="",
@@ -92,7 +86,6 @@ class CruiseLayout(Widget):
self.icbm_toggle,
self.scc_v_toggle,
self.scc_m_toggle,
self.accel_boost_toggle,
self.custom_acc_toggle,
self.custom_acc_short_increment,
self.custom_acc_long_increment,
@@ -143,8 +136,6 @@ class CruiseLayout(Widget):
self.icbm_toggle.set_description(new_desc)
self.icbm_toggle.show_description(True)
self.accel_boost_toggle.action_item.set_enabled(has_long)
if has_long or has_icbm:
self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
self.scc_v_toggle.action_item.set_enabled(True)
@@ -160,7 +151,6 @@ class CruiseLayout(Widget):
else:
has_icbm = has_long = False
self.icbm_toggle.action_item.set_enabled(False)
self.accel_boost_toggle.action_item.set_enabled(False)
self.icbm_toggle.set_description(tr(ONROAD_ONLY_DESCRIPTION))
show_custom_acc_desc = False
@@ -249,11 +249,7 @@ class ModelsLayout(Widget):
return {"name": name, "downloading": True, "progress": progress, "status_text": tr("verifying")}
if ds.downloading in statuses:
speed = max((p.speed for p in progresses), default=0.0)
speed_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
if any(p.resuming for p in progresses):
status_text = tr("resuming...") + f" {speed_text}"
else:
status_text = speed_text
status_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
return {"name": name, "downloading": True, "progress": progress, "status_text": status_text}
if statuses <= {ds.downloaded, ds.cached}:
return {"name": name, "text_color": ON_COLOR, "icon": "icons/checkmark.png"}
@@ -72,6 +72,11 @@ class VisualsLayout(Widget):
"It is the driver's responsibility to observe their environment and make decisions accordingly."),
None,
),
"TripSummary": (
lambda: tr("Trip Summary"),
tr("Show a summary of distance and engagement when you shift into park after a drive."),
None,
),
"TrueVEgoUI": (
lambda: tr("Speedometer: Always Display True Speed"),
tr("For applicable vehicles, always display the true vehicle current speed from wheel speed sensors."),
@@ -0,0 +1,199 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import time
import pyray as rl
from opendbc.car.structs import car
from openpilot.common.constants import CV
from openpilot.selfdrive.ui.onroad.augmented_road_view import BORDER_COLORS
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.widgets import Widget
AUTO_DISMISS = 20.
FADE_IN, FADE_OUT = 0.35, 0.3
LAT = (UIStatus.ENGAGED, UIStatus.LAT_ONLY)
LONG = (UIStatus.ENGAGED, UIStatus.LONG_ONLY)
GEAR = car.CarState.GearShifter
FORWARD = (GEAR.drive, GEAR.sport, GEAR.low, GEAR.brake, GEAR.eco, GEAR.manumatic)
class TripTracker:
def __init__(self):
self.reset()
def reset(self):
self.segments: list[list] = []
self.seconds = 0.
self.takeovers = 0
self._t = None
self._status = None
def update(self, driving: bool):
now = time.monotonic()
if not driving:
self._t = None
self._status = None
return
dt = 0. if self._t is None else min(now - self._t, 0.5)
self._t = now
self.seconds += dt
if self._status not in (None, UIStatus.DISENGAGED) and ui_state.status == UIStatus.DISENGAGED:
self.takeovers += 1
self._status = ui_state.status
d = max(ui_state.sm["carState"].vEgo, 0.) * dt
if d <= 0.:
return
if self.segments and self.segments[-1][0] == ui_state.status:
self.segments[-1][1] += d
else:
self.segments.append([ui_state.status, d])
def total(self, statuses=None) -> float:
return sum(m for s, m in self.segments if statuses is None or s in statuses)
class TripSummary:
def __init__(self):
self._trip = TripTracker()
self._dialog: TripSummaryDialog | None = None
self._gear = None
self._drove = False
gui_app.add_nav_stack_tick(self._tick)
def _tick(self):
gear = ui_state.sm["carState"].gearShifter if ui_state.started else None
self._trip.update(gear not in (None, GEAR.park))
self._drove |= gear in FORWARD
if gear == self._gear:
return
self._gear = gear
if gear not in (None, GEAR.park):
self._pop()
elif gear == GEAR.park and self._drove and ui_state.trip_summary and not ui_state.is_body and self._trip.total() > 0.:
self._dialog = TripSummaryDialog(self._trip, gui_app.get_active_widget(), self._pop)
gui_app.push_widget(self._dialog)
if gear is None:
self._trip.reset()
if gear in (None, GEAR.park):
self._drove = False
def _pop(self):
if self._dialog is not None and gui_app.get_active_widget() is self._dialog:
gui_app.pop_widget()
self._dialog = None
def _ease(t: float) -> float:
t = min(max(t, 0.), 1.)
return 1 - (1 - t) ** 3
def _fade(c, a: float) -> rl.Color:
r, g, b, al = c if isinstance(c, tuple) else (c.r, c.g, c.b, c.a)
return rl.Color(r, g, b, int(al * a))
class TripSummaryDialog(Widget):
def __init__(self, trip: TripTracker, below: Widget | None, on_closed):
super().__init__()
self._below = below
self._on_closed = on_closed
self._segments = segments = trip.segments
total = sum(m for _, m in segments)
def pct(statuses):
return 100 * sum(m for s, m in segments if s in statuses) / total
dist = total * (CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH) / 3600
dist_fmt = "{:.1f}" if dist < 9.95 else "{:.0f}"
self._stats = [
(dist, dist_fmt, "km" if ui_state.is_metric else "mi", "driven"),
(pct(LAT + LONG), "{:.0f}", "%", "engaged"),
(pct(LAT), "{:.0f}", "%", "lateral"),
(pct(LONG), "{:.0f}", "%", "longitudinal"),
]
mins = round(trip.seconds / 60)
self._details = [
(f"{mins // 60}h {mins % 60}" if mins >= 60 else f"{mins}", "min"),
(f"{trip.takeovers}", "takeover" if trip.takeovers == 1 else "takeovers"),
]
self._bold = gui_app.font(FontWeight.BOLD)
self._semi = gui_app.font(FontWeight.SEMI_BOLD)
self._normal = gui_app.font(FontWeight.NORMAL)
self._big = gui_app.big_ui()
self._shown_t = rl.get_time()
self._closing_t: float | None = None
self.set_click_callback(self.close)
def close(self):
if self._closing_t is None:
self._closing_t = rl.get_time()
def _render(self, rect: rl.Rectangle):
t = rl.get_time() - self._shown_t
if self._closing_t is None and t > AUTO_DISMISS:
self.close()
out = 0. if self._closing_t is None else (rl.get_time() - self._closing_t) / FADE_OUT
if out >= 1.:
self._on_closed()
return
bg = 1 - _ease(out)
a = _ease(t / FADE_IN) * bg
if self._big and self._below is not None and bg < 1.:
self._below.render(rect)
rl.draw_rectangle_rec(rect, _fade(rl.BLACK, bg))
k = rect.height / 1080 * (1.0 if self._big else 1.22)
pad = 120 * k
x, w = rect.x + pad, rect.width - 2 * pad
y = rect.y + (rect.height - 850 * k) / 2
rl.draw_text_ex(self._bold, "trip summary", rl.Vector2(x, y), int(150 * k), 0, _fade(rl.WHITE, a))
y += 260 * k
col_w = w / len(self._stats)
for i, (target, fmt, unit, label) in enumerate(self._stats):
cx = x + col_w * (i + 0.5)
v = measure_text_cached(self._bold, fmt.format(target), int(170 * k))
u = measure_text_cached(self._semi, unit, int(80 * k))
vx = cx - (v.x + 10 * k + u.x) / 2
rl.draw_text_ex(self._bold, fmt.format(target), rl.Vector2(vx, y), int(170 * k), 0, _fade(rl.WHITE, a))
rl.draw_text_ex(self._semi, unit, rl.Vector2(vx + v.x + 10 * k, y + 75 * k), int(80 * k), 0, _fade(rl.WHITE, a))
lb = measure_text_cached(self._normal, label, int(70 * k))
rl.draw_text_ex(self._normal, label, rl.Vector2(cx - lb.x / 2, y + 180 * k), int(70 * k), 0, _fade(rl.Color(200, 200, 200, 255), a))
y += 330 * k
bar = rl.Rectangle(x, y, w, 90 * k)
total = sum(m for _, m in self._segments)
sx = bar.x
for status, m in self._segments:
sw = bar.width * m / total
rl.draw_rectangle_rec(rl.Rectangle(sx, bar.y, min(sw + 1, bar.x + bar.width - sx), bar.height), _fade(BORDER_COLORS[status], a))
sx += sw
rl.draw_rectangle_rounded_lines_ex(bar, 1.0, 20, 30 * k, _fade(rl.BLACK, a))
y += (90 + 70) * k
size = int(90 * k)
parts = []
for i, (value, unit) in enumerate(self._details):
if i:
parts.append((" ", self._normal, None))
parts += [(value, self._bold, rl.WHITE), (" " + unit, self._normal, rl.Color(170, 170, 170, 255))]
widths = [measure_text_cached(f, t, size).x for t, f, _ in parts]
lx = x + (w - sum(widths)) / 2
for (text, font, color), tw in zip(parts, widths, strict=True):
if color is None:
rl.draw_circle(int(lx + tw / 2), int(y + size * 0.6), 12 * k, _fade(rl.Color(110, 110, 110, 255), a))
else:
rl.draw_text_ex(font, text, rl.Vector2(lx, y), size, 0, _fade(color, a))
lx += tw
@@ -267,8 +267,8 @@ class ModelsLayoutMici(NavScroller):
self.current_model_info.info_text.set_text(info_text)
if manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.failed:
self.current_model_info.info_header.set_text(tr("error"))
self.current_model_info.info_text.set_text(f"{manager.selectedBundle.internalName.lower()} | {tr('download failed')}")
self.current_model_info.info_header.set_text(tr("error") + self._download_progress)
self.current_model_info.info_text.set_text(tr("download failed"))
elif manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.downloading:
self.cancel_download_btn.set_visible(ui_state.params.get("ModelManager_DownloadRef") is not None)
@@ -277,7 +277,6 @@ class ModelsLayoutMici(NavScroller):
count = 0
verifying = False
canceling = False
resuming = False
speed = 0.0
for model in manager.selectedBundle.models:
count += 1
@@ -287,13 +286,12 @@ class ModelsLayoutMici(NavScroller):
progress += p.progress
verifying = verifying or p.status == custom.ModelManagerSP.DownloadStatus.verifying
canceling = canceling or p.canceling
resuming = resuming or p.resuming
speed = max(speed, p.speed) # only one file downloads at a time, so the non-zero speed is the live one
elif p.status in (custom.ModelManagerSP.DownloadStatus.downloaded,
custom.ModelManagerSP.DownloadStatus.cached):
progress += 100.0
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("resuming...") if resuming else tr("downloading")
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("downloading")
self.current_model_info.current_model_header.set_text(header)
self.cancel_download_btn.set_text(tr("cancel verification") if verifying else tr("cancel download"))
self.current_model_info.current_model_header._shimmer = True
@@ -48,6 +48,7 @@ class UIStateSP:
self.model_runner_tinygrad: bool = False
self.active_runner_tinygrad: bool = False
self.blindspot: bool = False
self.trip_summary: bool = True
self.chevron_metrics = None
self.custom_interactive_timeout: int = 0
self.developer_ui = None
@@ -185,6 +186,7 @@ class UIStateSP:
self.standstill_timer = self.params.get_bool("StandstillTimer")
self.sunnylink_enabled = self.params.get_bool("SunnylinkEnabled")
self.torque_bar = self.params.get_bool("TorqueBar")
self.trip_summary = self.params.get_bool("TripSummary")
self.enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
self.custom_torque_params = self.params.get_bool("CustomTorqueParams")
self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
+2
View File
@@ -0,0 +1,2 @@
SConscript(['common/transformations/SConscript'])
SConscript(['selfdrive/locationd/SConscript'])
@@ -0,0 +1,4 @@
Import('env')
transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
Export('transformations')
@@ -0,0 +1,100 @@
#define _USE_MATH_DEFINES
#include "sunnypilot/common/transformations/coordinates.hpp"
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>
double a = 6378137; // lgtm [cpp/short-global-name]
double b = 6356752.3142; // lgtm [cpp/short-global-name]
double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
double e1sq = 6.73949674228 * 0.001;
static Geodetic to_degrees(Geodetic geodetic){
geodetic.lat = RAD2DEG(geodetic.lat);
geodetic.lon = RAD2DEG(geodetic.lon);
return geodetic;
}
static Geodetic to_radians(Geodetic geodetic){
geodetic.lat = DEG2RAD(geodetic.lat);
geodetic.lon = DEG2RAD(geodetic.lon);
return geodetic;
}
ECEF geodetic2ecef(const Geodetic &geodetic) {
auto g = to_radians(geodetic);
double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2));
double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon);
double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon);
double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat);
return {x, y, z};
}
Geodetic ecef2geodetic(const ECEF &e) {
// Convert from ECEF to geodetic using Ferrari's methods
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
double x = e.x;
double y = e.y;
double z = e.z;
double r = sqrt(x * x + y * y);
double Esq = a * a - b * b;
double F = 54 * b * b * z * z;
double G = r * r + (1 - esq) * z * z - esq * Esq;
double C = (esq * esq * F * r * r) / (pow(G, 3));
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
double Q = sqrt(1 + 2 * esq * esq * P);
double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
double U = sqrt(pow((r - esq * r_0), 2) + z * z);
double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z);
double Z_0 = b * b * z / (a * V);
double h = U * (1 - b * b / (a * V));
double lat = atan((z + e1sq * Z_0) / r);
double lon = atan2(y, x);
return to_degrees({lat, lon, h});
}
LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) {
init_ecef << e.x, e.y, e.z;
auto g = to_radians(geodetic);
ned2ecef_matrix <<
-sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
-sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon),
cos(g.lat), 0, -sin(g.lat);
ecef2ned_matrix = ned2ecef_matrix.transpose();
}
NED LocalCoord::ecef2ned(const ECEF &e) {
Eigen::Vector3d ecef;
ecef << e.x, e.y, e.z;
Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef));
return {ned[0], ned[1], ned[2]};
}
ECEF LocalCoord::ned2ecef(const NED &n) {
Eigen::Vector3d ned;
ned << n.n, n.e, n.d;
Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef;
return {ecef[0], ecef[1], ecef[2]};
}
NED LocalCoord::geodetic2ned(const Geodetic &g) {
ECEF e = ::geodetic2ecef(g);
return ecef2ned(e);
}
Geodetic LocalCoord::ned2geodetic(const NED &n) {
ECEF e = ned2ecef(n);
return ::ecef2geodetic(e);
}
@@ -0,0 +1,43 @@
#pragma once
#include <eigen3/Eigen/Dense>
#define DEG2RAD(x) ((x) * M_PI / 180.0)
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
struct ECEF {
double x, y, z;
Eigen::Vector3d to_vector() const {
return Eigen::Vector3d(x, y, z);
}
};
struct NED {
double n, e, d;
Eigen::Vector3d to_vector() const {
return Eigen::Vector3d(n, e, d);
}
};
struct Geodetic {
double lat, lon, alt;
bool radians=false;
};
ECEF geodetic2ecef(const Geodetic &g);
Geodetic ecef2geodetic(const ECEF &e);
class LocalCoord {
public:
Eigen::Matrix3d ned2ecef_matrix;
Eigen::Matrix3d ecef2ned_matrix;
Eigen::Vector3d init_ecef;
LocalCoord(const Geodetic &g, const ECEF &e);
LocalCoord(const Geodetic &g) : LocalCoord(g, ::geodetic2ecef(g)) {}
LocalCoord(const ECEF &e) : LocalCoord(::ecef2geodetic(e), e) {}
NED ecef2ned(const ECEF &e);
ECEF ned2ecef(const NED &n);
NED geodetic2ned(const Geodetic &g);
Geodetic ned2geodetic(const NED &n);
};
@@ -0,0 +1,143 @@
#define _USE_MATH_DEFINES
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>
#include "sunnypilot/common/transformations/orientation.hpp"
#include "sunnypilot/common/transformations/coordinates.hpp"
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat) {
if (quat.w() > 0){
return quat;
} else {
return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z());
}
}
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) {
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
return ensure_unique(q);
}
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat) {
// TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
// return {euler(2), euler(1), euler(0)};
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
double theta = asin(asin_arg_clipped);
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
return {gamma, theta, psi};
}
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) {
return quat.toRotationMatrix();
}
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) {
return ensure_unique(Eigen::Quaterniond(rot));
}
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) {
return quat2rot(euler2quat(euler));
}
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) {
return quat2euler(rot2quat(rot));
}
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) {
return euler2rot({roll, pitch, yaw});
}
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle) {
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(angle, axis);
return q.toRotationMatrix();
}
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) {
/*
Using Rotations to Build Aerospace Coordinate Systems
Don Koks
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
*/
LocalCoord converter = LocalCoord(ecef_init);
Eigen::Vector3d zero = ecef_init.to_vector();
Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0;
Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0;
Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0;
Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1;
Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1;
Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1;
Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2;
Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2;
x0 = Eigen::Vector3d(1, 0, 0);
y0 = Eigen::Vector3d(0, 1, 0);
z0 = Eigen::Vector3d(0, 0, 1);
double psi = atan2(x3.dot(y0), x3.dot(x0));
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
y2 = rot(z0, psi) * y0;
z2 = rot(y2, theta) * z0;
double phi = atan2(y3.dot(z2), y3.dot(y2));
return {phi, theta, psi};
}
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) {
/*
Using Rotations to Build Aerospace Coordinate Systems
Don Koks
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
*/
LocalCoord converter = LocalCoord(ecef_init);
Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0);
Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0);
Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1);
Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0;
Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0;
Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0;
Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1;
Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1;
Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1;
Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2;
Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2;
Eigen::Vector3d zero = ecef_init.to_vector();
x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
double psi = atan2(x3.dot(y0), x3.dot(x0));
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
y2 = rot(z0, psi) * y0;
z2 = rot(y2, theta) * z0;
double phi = atan2(y3.dot(z2), y3.dot(y2));
return {phi, theta, psi};
}
@@ -0,0 +1,17 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include "sunnypilot/common/transformations/coordinates.hpp"
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat);
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler);
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat);
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat);
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot);
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler);
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot);
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw);
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle);
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose);
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose);
+1 -1
View File
@@ -139,7 +139,7 @@ class ModularAssistiveDrivingSystem:
self.transition_paused_state()
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
if CS.brakePressed or CS.regenBraking or self.pedal_pressed_non_gas_pressed(CS):
if self.pedal_pressed_non_gas_pressed(CS):
self.transition_paused_state()
self.events.remove(EventName.preEnableStandstill)
@@ -107,22 +107,6 @@ class TestShouldSilentLkasEnable(OpenpilotTestCase):
# pause
class TestPauseMode(OpenpilotTestCase):
def test_pauses_at_standstill_brake_already_held(self, mocker):
mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
mads.state_machine.state = State.enabled
mads.enabled = True
mads.active = True
sd.CS_prev = make_car_state(brake_pressed=True, standstill=True)
sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
cs = make_car_state(brake_pressed=True, standstill=True)
run_frames(mads, sd, cs, n=199)
mads.update(cs)
assert not sd.events_sp.has(EventNameSP.controlsMismatchLateral)
assert mads.state_machine.state == State.paused
assert mads.lateral_mismatch_counter == 0
def test_stays_paused_at_standstill_brake_held(self, mocker):
mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
mads.state_machine.state = State.enabled
@@ -19,12 +19,12 @@ class BaseMapData(ABC):
def __init__(self):
self.params = Params()
self.sm = messaging.SubMaster(['gpsLocationSP'])
self.sm = messaging.SubMaster(['liveLocationKalman'])
self.pm = messaging.PubMaster(['liveMapDataSP'])
self.localizer_valid = False
self.last_bearing = None
self.last_position = coordinate_from_param("LastGPSPositionFused", self.params)
self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
@abstractmethod
def update_location(self) -> None:
@@ -47,7 +47,7 @@ class BaseMapData(ABC):
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
mapd_sp_send = messaging.new_message('liveMapDataSP')
mapd_sp_send.valid = self.sm['gpsLocationSP'].gpsOK
mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
live_map_data = mapd_sp_send.liveMapDataSP
live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
@@ -5,8 +5,10 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
from openpilot.sunnypilot.navd.helpers import Coordinate
@@ -18,12 +20,12 @@ class OsmMapData(BaseMapData):
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
def update_location(self) -> None:
location = self.sm['gpsLocationSP']
self.localizer_valid = self.sm.valid['gpsLocationSP'] and location.hasFix
location = self.sm['liveLocationKalman']
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
if self.localizer_valid:
self.last_bearing = location.bearingDeg
self.last_position = Coordinate(location.latitude, location.longitude)
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
if self.last_position is None:
return
@@ -5,9 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
@@ -18,26 +20,28 @@ class TestOsmMapData(OpenpilotTestCase):
LATITUDE = 32.875134
LONGITUDE = -117.209777
ALTITUDE = 42.0
BEARING = 90.0
def setup_method(self):
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else Params()
self.mem_params.remove("LastGPSPosition")
self.map_data = OsmMapData()
self.pm = messaging.PubMaster(['gpsLocationSP'])
self.pm = messaging.PubMaster(['liveLocationKalman'])
def location_message(self, has_fix: bool):
msg = messaging.new_message('gpsLocationSP', valid=has_fix)
location = msg.gpsLocationSP
location.hasFix = has_fix
location.latitude, location.longitude, location.bearingDeg = self.LATITUDE, self.LONGITUDE, self.BEARING
def location_message(self, status: log.LiveLocationKalman.Status, position_valid: bool):
msg = messaging.new_message('liveLocationKalman', valid=True)
location = msg.liveLocationKalman
location.status = status
location.positionGeodetic = {'value': [self.LATITUDE, self.LONGITUDE, self.ALTITUDE], 'valid': position_valid}
location.calibratedOrientationNED = {'value': [0.0, 0.0, math.radians(self.BEARING)], 'valid': True}
return msg
def send_location(self, has_fix: bool) -> None:
def send_location(self, status: log.LiveLocationKalman.Status, position_valid: bool) -> None:
# the first exchange synchronizes the PUB/SUB connection, the second one is actually received
self.pm.send('gpsLocationSP', self.location_message(has_fix))
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
self.map_data.sm.update(1000)
self.pm.send('gpsLocationSP', self.location_message(has_fix))
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
self.map_data.sm.update(1000)
def read_position(self) -> dict:
@@ -46,7 +50,7 @@ class TestOsmMapData(OpenpilotTestCase):
return json.loads(value)
def test_writes_position_for_mapd(self):
self.send_location(True)
self.send_location(log.LiveLocationKalman.Status.valid, True)
self.map_data.tick()
position = self.read_position()
@@ -55,10 +59,10 @@ class TestOsmMapData(OpenpilotTestCase):
self.assertAlmostEqual(position['bearing'], self.BEARING, delta=1e-6)
def test_invalid_localizer_keeps_last_known_position(self):
self.send_location(True)
self.send_location(log.LiveLocationKalman.Status.valid, True)
self.map_data.tick()
self.send_location(False)
self.send_location(log.LiveLocationKalman.Status.uninitialized, False)
self.map_data.tick()
position = self.read_position()
+4 -12
View File
@@ -17,25 +17,16 @@ def main():
parser.add_argument("--frame", required=True, help="width,height,stride,y_height,uv_height,buffer_size")
parser.add_argument("--warp-to", required=True, help="model input WxH")
parser.add_argument("--output", required=True)
parser.add_argument("--split-frames", action="store_true", help="one input per camera (frame, big_frame), bound to the camera buffers by pointer")
args = parser.parse_args()
from examples.openpilot.compile_warp import make_frame_prepare, parse_frame, parse_size
from tinygrad import Device, Tensor, TinyJit
frame = parse_frame(args.frame)
warp = make_frame_prepare(frame, *parse_size(args.warp_to))
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
rng = np.random.default_rng(42)
frames = rng.integers(0, 256, (2, frame.size), dtype=np.uint8)
m_inv = Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
if args.split_frames:
run = TinyJit(lambda frame, big_frame, M_inv: Tensor.stack(warp(frame, M_inv[0]), warp(big_frame, M_inv[1])), prune=True)
inputs = {"frame": Tensor(frames[0], device=Device.DEFAULT).realize(), "big_frame": Tensor(frames[1], device=Device.DEFAULT).realize(), "M_inv": m_inv}
specs = {"frame": ((frame.size,), "|u1", Device.DEFAULT), "big_frame": ((frame.size,), "|u1", Device.DEFAULT),
"M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
else:
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
inputs = {"input_frame": Tensor(frames, device=Device.DEFAULT).realize(), "M_inv": m_inv}
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
inputs = {"input_frame": Tensor(rng.integers(0, 256, (2, frame.size), dtype=np.uint8), device=Device.DEFAULT).realize(),
"M_inv": Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()}
with record_kernel_linear() as recorded:
run(**inputs)
run(**inputs)
@@ -43,6 +34,7 @@ def main():
assert len(recorded) == 1, f"expected one captured kernel list, got {len(recorded)}"
part = serialize_jit(run, recorded[0], check)
PlainJit(*part)
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
write_bundle(args.output, {"run": part}, {"input_specs": specs, "metadata": {}})
print(f"Saved warp to {args.output} ({len(part[0]['calls'])} kernels)")
@@ -66,75 +66,47 @@ def _patched(buf, topo) -> tuple[bytearray, dict]:
return blob, rows
def _packed_args(program) -> dict[int, int]:
from tinygrad.device import TinyELF
params = getattr(program.arg, "params", None) or ()
return {off: p.arg.slot for (off, _), p in zip(TinyELF.iter_sig(tuple((None, p.addrspace, p.dtype, ()) for p in params)), params, strict=True)}
def _outer_slot(u, packed: dict[int, int]) -> int | None:
from tinygrad.uop.ops import Ops
if u.op is Ops.PARAM:
return None if packed else u.arg.slot
if u.op.name == "REFPARAM":
return u.arg.slot
if packed and u.op is Ops.LOAD and u.src[0].op is Ops.INDEX and _root(u.src[0].src[0]).op is Ops.PARAM:
index = _evaluate(u.src[0].src[1], {}, packed)
return None if index is None else packed.get(index * u.dtype.itemsize)
return None
def _evaluate(u, env, packed: dict[int, int]):
def _evaluate(u, env):
from tinygrad.uop.ops import GroupOp, Ops, exec_alu
if u.op in (Ops.AFTER, Ops.CAST, Ops.BITCAST):
return _evaluate(u.src[0], env, packed)
return _evaluate(u.src[0], env)
if u.op is Ops.CONST:
return u.arg
if u.op is Ops.RANGE:
return env.get(u)
if (slot := _outer_slot(u, packed)) is not None:
return env.get(slot)
if u.op in (Ops.PARAM, Ops.RANGE):
return env.get(u.arg.slot if u.op is Ops.PARAM else u)
if u.op is Ops.LOAD:
table, index = _root(u.src[0].src[0]), _evaluate(u.src[0].src[1], env, packed)
table, index = _root(u.src[0].src[0]), _evaluate(u.src[0].src[1], env)
return struct.unpack_from(u.dtype.fmt, table.arg, index * u.dtype.itemsize)[0] if table.op is Ops.BINARY and index is not None else None
if u.op in GroupOp.ALU and None not in (values := [_evaluate(x, env, packed) for x in u.src]):
if u.op in GroupOp.ALU and None not in (values := [_evaluate(x, env) for x in u.src]):
return exec_alu(u.op, u.dtype, values, *([False] if "truncate_output" in inspect.signature(exec_alu).parameters else []))
return None
def _input_pokes(submit, kernargs) -> dict[int, tuple[int, int]]:
from tinygrad.uop.ops import Ops
outer, packed = dict(enumerate(submit.src[1:])), _packed_args(submit.src[0])
outer = dict(enumerate(submit.src[1:]))
kernargs_params = {i for i, a in outer.items() if a.op is Ops.GETADDR and _root(a.src[0]) is kernargs}
inputs = {i: _root(a.src[0]).arg.slot for i, a in outer.items() if a.op is Ops.GETADDR and _root(a.src[0]).op is Ops.PARAM}
pokes = {}
for call in (u for u in submit.src[0].src[0].toposort() if u.op is Ops.CALL and len(u.src) > 3):
_, address, value, *_ = call.src[1:]
used = list({slot for x in value.toposort() if (slot := _outer_slot(x, packed)) is not None} & inputs.keys())
if len(used) != 1 or not kernargs_params & {_outer_slot(x, packed) for x in address.toposort()}:
used = list({x.arg.slot for x in value.toposort() if x.op is Ops.PARAM} & inputs.keys())
if len(used) != 1 or not kernargs_params & {x.arg.slot for x in address.toposort() if x.op is Ops.PARAM}:
continue
ranges = list(dict.fromkeys(x for x in (*address.toposort(), *value.toposort()) if x.op is Ops.RANGE))
for trip in itertools.product(*[range(int(r.vmax) + 1) for r in ranges]):
env = dict.fromkeys(outer, 0) | dict(zip(ranges, trip, strict=True))
if (offset := _evaluate(address, env, packed)) % 8 == 0:
pokes[offset] = (inputs[used[0]], _evaluate(value, env, packed))
if (offset := _evaluate(address, env)) % 8 == 0:
pokes[offset] = (inputs[used[0]], _evaluate(value, env))
return pokes
def _elf(image: bytes, rodata: int, entry: int) -> bytes:
spans = [(".text", 0, rodata), (".rodata", rodata, len(image))] if entry < rodata else \
[(".data", 0, rodata), (".rodata", rodata, entry), (".text", entry, len(image))]
sections, names = [(0, 0, 0, 0, b"")], b"\0"
for name, start, end in (span for span in spans if span[1] < span[2]):
sections.append((len(names), 1, 6 if name == ".text" else 2, ALIGNMENT + start, image[start:end]))
names += name.encode() + b"\0"
sections.append((len(names), 3, 0, 0, names + b".shstrtab\0"))
offset, data, headers = 64, b"", []
for name, kind, flags, address, content in sections:
headers.append(struct.pack("<IIQQQQIIQQ", name, kind, flags, address, offset, len(content), 0, 0, 1, 0))
def _elf(image: bytes, rodata: int) -> bytes:
names, offset, data, headers = b"\0.text\0.rodata\0.shstrtab\0", 64, b"", []
for name, kind, address, content in [(0, 0, 0, b""), (1, 1, ALIGNMENT, image[:rodata]), (7, 1, ALIGNMENT + rodata, image[rodata:]), (15, 3, 0, names)]:
headers.append(struct.pack("<IIQQQQIIQQ", name, kind, 2 if kind == 1 else 0, address, offset, len(content), 0, 0, 1, 0))
data, offset = data + content, offset + len(content)
header = struct.pack("<HHIQQQIHHHHHH", 1, 0xe0, 1, 0, 0, offset, 0, 64, 0, 0, 64, len(sections), len(sections) - 1)
return b"\x7fELF\x02\x01\x01" + bytes(9) + header + data + b"".join(headers)
return b"\x7fELF\x02\x01\x01" + bytes(9) + struct.pack("<HHIQQQIHHHHHH", 1, 0xe0, 1, 0, 0, offset, 0, 64, 0, 0, 64, 4, 3) + data + b"".join(headers)
def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
@@ -183,7 +155,7 @@ def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
if image_alloc not in programs:
image = bytes(_patched(image_alloc, topo)[0])
rodata = next(r for r in range(0, len(image) - 24, 64) if r + struct.unpack_from("<q", image, r + 16)[0] == entry)
programs[image_alloc] = blob.add(_elf(image, rodata, entry))
programs[image_alloc] = blob.add(_elf(image, rodata))
start, size = _view(arguments.src[0])
args = []
for slot in range(start, start + unwrap(size), 8):
@@ -12,8 +12,6 @@ from functools import partial
from openpilot.common.basedir import BASEDIR
from openpilot.sunnypilot.modeld_v2.helpers import _dtype, read_parts
SPLIT_FRAMES = ("big_frame", "frame")
def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]:
offsets, offset = {}, 0
@@ -41,30 +39,29 @@ def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
(model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
frame_specs = {s["name"]: s for s in warp["inputs"] if s["name"] in SPLIT_FRAMES}
offsets, packed = pack_layout(host, 0 if frame_specs else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2
offsets, packed = pack_layout(host, frame_size)
shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8"))
def view(name: str, nbytes: int, dtype: str) -> dict:
return {"buffer": upload, "offset": offsets[name], "nbytes": nbytes, "dtype": dtype}
outputs = sorted(s["slot"] for s in model["inputs"] if s["name"] is None)
names = sorted(["host", *states, *frame_specs])
names = sorted(["host", *states])
slots = {s["slot"]: names.index(s["name"]) for s in model["inputs"] if s["name"] in states} | \
{slot: len(names) + i for i, slot in enumerate(outputs)}
returned = warp["returns"]["tensors"][0]
model_inputs = {s["slot"]: {"input": slots[s["slot"]]} if s["slot"] in slots else
{k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + len(model["buffers"])} if s["name"] == "new_img" else
view(s["name"], s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in model["inputs"]}
warp_inputs = {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in warp["inputs"]}
warp_inputs = {s["slot"]: view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"])
for s in warp["inputs"]}
def remap(calls: list, inputs: dict, buffers: int, lib: int) -> list:
return [call | ({"lib": [call["lib"][0] + lib, call["lib"][1] + lib]} if "lib" in call else {}) |
{"args": [dict(inputs[a["input"]]) if "input" in a else a | {"buffer": a["buffer"] + buffers} for a in call["args"]]} for call in calls]
header = {"input_names": names, "returns": {"kind": "NoneType", "tensors": []}, "check": None,
"inputs": [{"slot": names.index("host"), "name": "host", "dtype": u8, "numel": packed, "device": "NPY"}] +
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots] +
[s | {"slot": names.index(name)} for name, s in frame_specs.items()],
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots],
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + shift, b["init"][1] + shift]} if "init" in b else {}) for b in warp["buffers"]] +
[{"nbytes": packed, "device": model["inputs"][0]["device"], "dtype": u8}],
"calls": [{"kind": "copy", "args": [{"buffer": upload, "offset": 0, "nbytes": packed, "dtype": u8}, {"input": names.index("host")}]}] +
+3 -20
View File
@@ -264,38 +264,21 @@ def _program_uop(call: dict, lib: bytes):
parameter = UOp.param(call["globals"][compact_slot], _dtype(dtype), math.prod(shape))
image = (shape[0], shape[1]) if len(shape) == 3 and shape[2] == 4 else None
parameters.append(parameter.replace(arg=_fields(parameter.arg, name=name, image=image)))
info = _fields(ProgramInfo, global_size=tuple(call["global_size"]), local_size=tuple(call["local_size"]), params=tuple(parameters),
globals=tuple(call["globals"]), outs=tuple(call["outs"]), ins=tuple(call["ins"]), target=_fields(Target, **call["target"]))
info = _fields(ProgramInfo, global_size=tuple(call["global_size"]), local_size=tuple(call["local_size"]), globals=tuple(call["globals"]),
outs=tuple(call["outs"]), ins=tuple(call["ins"]), target=_fields(Target, **call["target"]))
return UOp(Ops.PROGRAM, src=(UOp(Ops.SINK, arg=KernelInfo(name=call["name"])), UOp(Ops.LINEAR, src=tuple(parameters)),
UOp(Ops.SOURCE, arg=""), UOp(Ops.BINARY, arg=lib)), arg=info)
def _input_tensors(kwargs: dict) -> list: # in _prepare_jit_inputs' order
from tinygrad import Tensor
tensors = [t for _, t in sorted(kwargs.items()) if t.__class__ is Tensor]
for value in kwargs.values():
tensors += [t for t in (value.values() if isinstance(value, dict) else value if isinstance(value, (tuple, list)) else ())
if t.__class__ is Tensor and not any(t is y for y in tensors)]
return tensors
class _Jit:
def __init__(self, captured, input_names: list, buffers: list):
self.captured, self.input_names, self.buffers = captured, input_names, buffers
def __call__(self, *args, **kwargs):
import tinygrad.engine.jit as jit
from tinygrad.uop.ops import Ops
# same names, no variables: the binding is the tensors' own uops, without _prepare_jit_inputs' graph rewrites
if not args and self.__dict__.get("_names") == sorted(kwargs):
tensors = _input_tensors(kwargs)
if all(t.uop.is_realized for t in tensors):
return self.captured([t.uop.src[0] if t.uop.op is Ops.UNSHARD else t.uop for t in tensors], {})
input_uops, var_vals, names, _ = jit._prepare_jit_inputs(args, kwargs)
if names != self.input_names:
raise ValueError(f"expected inputs {self.input_names}, got {names}")
if not args and not var_vals:
self._names = sorted(kwargs)
return self.captured(input_uops, var_vals)
@@ -328,7 +311,7 @@ class PlainJit(_Jit):
calls.append(_program_uop(call, bytes(blob[slice(*call["lib"])])).call(*arguments))
else:
calls.append(arguments[0].store_call(arguments[1]))
# allocate gpu buffers
# stand-ins for JIT inputs: copy staging to GPU is decided from the input buffers, the real ones bind per call
stand_ins = {slot: UOp.from_buffer(Buffer(_device(spec["device"], device_map), spec["numel"] * _dtype(spec["dtype"]).itemsize).ensure_allocated(),
_dtype(spec["dtype"])) for slot, spec in inputs.items()}
input_uops = tuple(stand_ins.get(slot, next(iter(stand_ins.values()), None)) for slot in range(max(stand_ins, default=-1) + 1))
@@ -11,7 +11,7 @@ import pickle
import numpy as np
from openpilot.common.basedir import BASEDIR
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout, SPLIT_FRAMES
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout
from openpilot.sunnypilot.modeld_v2.helpers import load_model
from openpilot.sunnypilot.modeld_v2.compile_modeld import (POLICY_INPUTS, derive_frame_skip,
make_split_input_queues, make_supercombo_input_queues)
@@ -169,18 +169,16 @@ class NativeTinygradAdapter(BaseModelAdapter):
self.warp_frame_size = self.frame_copy_size
self._init_common()
self.run_model = self.jits['run']
self.split = [name for name in SPLIT_FRAMES if name in self.run_model.input_names]
self.frame_inputs: dict[str, Tensor] = {}
self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
offsets, packed = pack_layout(shapes, 0 if self.split else self.warp_frame_size)
offsets, packed = pack_layout(shapes, self.warp_frame_size)
self.packed_input = np.zeros(packed, dtype=np.uint8)
self.host = Tensor(self.packed_input, device='NPY')
self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name])
for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()}
self.frames = self.packed_input[offsets['frames']:].reshape(2, -1)
self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size)
self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()}
for name, next_name in self.state_pairs.items():
@@ -189,29 +187,17 @@ class NativeTinygradAdapter(BaseModelAdapter):
def copy_frames(self, bufs):
for i, key in enumerate(self._vision_input_names):
if key not in bufs:
continue
if not self.split:
if key in bufs:
data = bufs[key].data if hasattr(bufs[key], 'data') else bufs[key]
np.copyto(self.frames[i], np.frombuffer(data, dtype=np.uint8, count=self.warp_frame_size))
elif (name := ('frame', 'big_frame')[i]) in self.split:
if isinstance(bufs[key], np.ndarray): # cached memory: the GPU would read stale lines in place
self.frame_inputs[name] = Tensor(np.ascontiguousarray(bufs[key][:self.warp_frame_size]), device=self.model_device).realize()
continue
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data # visionipc buffers are coherent
if (key, ptr) not in self._blob_cache:
self._blob_cache[(key, ptr)] = Tensor.from_blob(ptr, (self.warp_frame_size,), dtype='uint8', device=self.model_device)
self.frame_inputs[name] = self._blob_cache[(key, ptr)]
def reset_warmup_buffers(self) -> None:
self.packed_input[:] = 0
self.frame_inputs.clear()
self._blob_cache.clear()
for key in self.state_pairs:
self.input_queues[key].assign(0).realize()
def run(self):
self.run_model(host=self.host, **self.input_queues, **self.frame_inputs, output_buffers=self.outputs)
self.run_model(host=self.host, **self.input_queues, output_buffers=self.outputs)
return self.outputs['outputs']
+1 -1
View File
@@ -242,7 +242,7 @@ class ModelState(ModelStateBase):
desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
else:
desired_accel = model_output['action'][0, 1]
desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
if blendful is not None:
desired_curvature += blendful(desired_curvature)
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d776c72125156b5dd3eb59768409401e28c8a52a3a619cd3b9f02c10a156fda6
size 3699072
oid sha256:4b22fd6bcb3a3e8a642e6866449dd9c30c12e694ff0741cad9e34e4b00d8c5fa
size 3698799
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a3f13ed12e087817a86afd48b2496c66c4f61a1925065b676b2945e81fabaacd
size 7924864
oid sha256:9a17fd6acece4fab23784ef0f3f8bba28a049e9599e5e9e5ee7b36e4b1d24a6f
size 7924847
@@ -6,7 +6,6 @@ See the LICENSE.md file in the root directory for more details.
"""
import os
import re
import struct
import sys
import tempfile
import unittest
@@ -26,8 +25,8 @@ if sys.platform == "darwin" and os.environ.get("DEV", "").startswith("MOCKUSB"):
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.modeld_v2 import convert_upstream_pkl
from openpilot.sunnypilot.modeld_v2.convert_upstream_pkl import _elf, convert
from openpilot.sunnypilot.modeld_v2.helpers import ALIGNMENT, PlainJit, load_model
from openpilot.sunnypilot.modeld_v2.convert_upstream_pkl import convert
from openpilot.sunnypilot.modeld_v2.helpers import PlainJit, load_model
from openpilot.sunnypilot.modeld_v2.plain_artifact_compiler import capture_frame
IMG, DESIRE, STATE, OUT = (6, 8, 8), (8,), (4, 16), (1, 32)
@@ -120,18 +119,3 @@ class TestConvertUpstreamPkl(OpenpilotTestCase):
bundle = load_model(out, cache=False, verify=True)
assert bundle["metadata"] == {"name": "tiny"} and bundle["input_specs"] == self.input_specs and bundle["output_specs"] == self.output_specs
self.assert_matches_eager(bundle["run"], 6)
class TestKernelElf(OpenpilotTestCase):
def test_sections_follow_the_code(self):
from tinygrad.runtime.support.elf import elf_loader
code = bytes(range(1, 129))
for rodata, entry in ((len(code), 0), (0, 256)):
image = bytearray(code + bytes(64) if entry < rodata else bytes(256) + code)
struct.pack_into("<q", image, rodata + 16, entry - rodata)
loaded, sections, _ = elf_loader(_elf(bytes(image), rodata, entry))
headers = {section.name: section.header for section in sections}
assert bytes(loaded[ALIGNMENT:]) == bytes(image), "the uploaded program differs from the pickle's"
assert headers[".rodata"].sh_addr == ALIGNMENT + rodata, "the loader reads the kernel descriptor at .rodata"
text = headers[".text"]
assert text.sh_addr <= ALIGNMENT + entry < text.sh_addr + text.sh_size, "the amd mock sizes the code from the entry to the end of .text"
@@ -144,21 +144,6 @@ class TestPlainArtifact(OpenpilotTestCase):
for name in ("outputs", "next_state_feat_q"):
assert np.array_equal(inputs["output_buffers"][name].numpy(), expected["output_buffers"][name].numpy()), name
split_path = os.path.join(self.tmp.name, "warp_split_tinygrad.pkl")
with mock.patch.object(sys, "argv", ["compile_warp", "--frame", FRAME, "--warp-to", f"{MODEL_W}x{MODEL_H}", "--output", split_path, "--split-frames"]):
compile_warp.main()
split = load_model(path, cache=False, extend=functools.partial(frame_part, split_path))
assert split["run"].input_names == ["big_frame", "frame", "host", "state_feat_q"]
offsets, packed = pack_layout(HOST, 0)
host = np.zeros(packed, np.uint8)
for name, shape in ({"tfm": (2, 3, 3)} | HOST).items():
np.ndarray(shape, np.float32, host, offsets[name])[...] = tfm.numpy() if name == "tfm" else inputs[name].numpy()
out = {"outputs": Tensor.zeros(OUT).contiguous().realize(), "next_state_feat_q": Tensor.zeros(STATE).contiguous().realize()}
split["run"](host=Tensor(host, device="NPY"), frame=Tensor(frames.numpy()[0]).realize(), big_frame=Tensor(frames.numpy()[1]).realize(),
state_feat_q=expected["state_feat_q"], output_buffers=out)
for name in ("outputs", "next_state_feat_q"):
assert np.array_equal(out[name].numpy(), expected["output_buffers"][name].numpy()), name
def test_tinygrad_api_the_loader_reads(self):
import tinygrad.engine.jit as jit
from tinygrad.device import Buffer, TinyELF
+29 -120
View File
@@ -6,7 +6,6 @@ See the LICENSE.md file in the root directory for more details.
"""
import asyncio
import json
import os
import re
import threading
@@ -29,11 +28,8 @@ DOWNLOAD_TIMEOUT = (30, 30)
# HuggingFace throttles each connection to ~1-2 MB/s but not the connection count; 12 saturates the device link
MAX_CONCURRENT_PIECES = 12
PIECE_SIZE = 8 * 1024 * 1024
PIECE_ATTEMPTS = 3 # per piece, before the transfer as a whole is retried
PIECE_ATTEMPTS = 3
PIECE_BACKOFF = 1.0 # seconds, growing linearly: waits 1 s then 2 s between a piece's three attempts
DOWNLOAD_ATTEMPTS = 4 # whole-transfer attempts, each resuming from the sidecar
RETRY_BACKOFF = 2.0 # seconds, doubling: waits 2, 4 then 8 s between the four transfer attempts
RETRY_POLL = 0.25 # how often a backoff checks for a cancel
TRANSIENT_HTTP_STATUS = frozenset({408, 429, 500, 502, 503, 504})
REPORT_INTERVAL = 0.5 # seconds between progress publications
# byte ranges count encoded bytes but iter_content yields decoded ones, so ask for an unencoded body
@@ -53,69 +49,26 @@ def _is_transient(e: BaseException) -> bool:
class _Progress:
def __init__(self, done: set[int], done_bytes: int):
self._bytes = done_bytes
self._done = set(done)
def __init__(self):
self._bytes = 0
self._lock = threading.Lock()
def add_bytes(self, n: int) -> None:
with self._lock:
self._bytes += n
def mark_done(self, index: int) -> None:
def snapshot(self) -> int:
with self._lock:
self._done.add(index)
def snapshot(self) -> tuple[int, set[int]]:
with self._lock:
return self._bytes, set(self._done)
return self._bytes
def _piece_ranges(total: int, piece_size: int) -> list[tuple[int, int]]:
return [(start, min(start + piece_size, total)) for start in range(0, total, piece_size)]
def _sidecar_path(path: str) -> str:
return f"{path}.download"
def _download_in_progress(path: str) -> bool:
return os.path.isfile(_sidecar_path(path))
def _save_resume_state(path: str, layout: dict, done: set[int]) -> None:
tmp = _sidecar_path(path) + ".tmp"
with open(tmp, "w") as f:
json.dump({**layout, "done": sorted(done)}, f)
f.flush()
os.fsync(f.fileno())
os.replace(tmp, _sidecar_path(path)) # a power loss leaves the old sidecar or the new one, never a torn file
dir_fd = os.open(os.path.dirname(path) or ".", os.O_RDONLY)
try:
os.fsync(dir_fd)
finally:
os.close(dir_fd)
def _remove_download_state(path: str) -> None:
for stale in (_sidecar_path(path), _sidecar_path(path) + ".tmp"):
if os.path.isfile(stale):
os.remove(stale)
def _prepare_target(path: str, layout: dict) -> set[int]:
total = layout["total"]
try:
with open(_sidecar_path(path)) as f:
state = json.load(f)
if isinstance(state, dict) and all(state.get(k) == v for k, v in layout.items()) and os.path.getsize(path) == total:
return {i for i in state.get("done", []) if isinstance(i, int)}
except (OSError, ValueError, TypeError): # missing, unreadable or malformed sidecar: start over
pass
_save_resume_state(path, layout, set())
def _prepare_target(path: str, total: int) -> None:
with open(path, "wb") as f:
f.truncate(total)
return set()
def _content_range_total(response: requests.Response) -> int | None:
@@ -157,9 +110,6 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
f.truncate() # unknown length: the body defines the file size
if end is not None and written != end - start:
raise requests.exceptions.ConnectionError(f"short read: {written} of {end - start} bytes")
f.flush()
os.fsync(f.fileno()) # before it is recorded, so a power loss never leaves the sidecar naming unsynced bytes
progress.mark_done(index)
return
except requests.RequestException as e:
progress.add_bytes(-written) # the piece restarts from scratch
@@ -170,11 +120,6 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
raise DownloadCancelled("Download cancelled") from None
def _remove_file(path: str) -> None:
if os.path.isfile(path):
os.remove(path)
class ModelManagerSP:
"""Manages model downloads and status reporting"""
@@ -241,12 +186,11 @@ class ModelManagerSP:
else:
eta = self._calculate_eta(artifact.fileName, progress)
self._set_progress(artifact, custom.ModelManagerSP.DownloadStatus.downloading, progress, eta, speed)
artifact.downloadProgress.resuming = False # shown only for the first report, when the bar jumps to the resumed point
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, path: str, layout: dict,
started: tuple[int, set[int], float]) -> None:
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, total: int,
started: tuple[int, float]) -> None:
pending = set(tasks)
last_bytes, saved, last_time = started
last_bytes, last_time = started
speed = 0.0
while pending:
done, pending = await asyncio.wait(pending, timeout=REPORT_INTERVAL)
@@ -255,17 +199,14 @@ class ModelManagerSP:
if self._download_interrupted():
raise DownloadCancelled("Download cancelled")
now = time.monotonic()
done_bytes, done_pieces = progress.snapshot()
if done_pieces != saved:
_save_resume_state(path, layout, done_pieces)
saved = done_pieces
done_bytes = progress.snapshot()
if (dt := now - last_time) > 0:
instant = max(0.0, (done_bytes - last_bytes) / dt) # bytes/s this tick; clamped since retried pieces can lower the byte count
speed = instant if speed == 0 else SPEED_SMOOTHING * speed + (1 - SPEED_SMOOTHING) * instant
if speed < SPEED_FLOOR: # a stall decays toward 0 without reaching it, and a near-0 speed overflows the eta
speed = 0.0
last_time, last_bytes = now, done_bytes
self._publish_progress(artifact, done_bytes, layout["total"], speed)
self._publish_progress(artifact, done_bytes, total, speed)
async def _download_file(self, url: str, path: str, artifact) -> None:
self._download_start_times[artifact.fileName] = time.monotonic()
@@ -273,18 +214,16 @@ class ModelManagerSP:
total, ranged = await loop.run_in_executor(None, _probe_size, url)
pieces: list[tuple[int, int | None]] = list(_piece_ranges(total, PIECE_SIZE)) if ranged else [(0, None)]
layout = {"total": total, "piece_size": PIECE_SIZE, "ranged": ranged, "sha256": artifact.downloadUri.sha256}
done = await loop.run_in_executor(None, _prepare_target, path, layout)
resumed_bytes = sum(end - start for i, (start, end) in enumerate(pieces) if i in done and end is not None)
progress = _Progress(done, resumed_bytes) # the bar and eta start from the resumed pieces
await loop.run_in_executor(None, _prepare_target, path, total)
progress = _Progress()
cancel = threading.Event()
started = *progress.snapshot(), time.monotonic() # before any worker can land bytes
started = progress.snapshot(), time.monotonic() # before any worker can land bytes
with ThreadPoolExecutor(max_workers=MAX_CONCURRENT_PIECES) as pool:
tasks = [loop.run_in_executor(pool, _fetch_piece, url, path, i, start, end, progress, cancel, self._block_size)
for i, (start, end) in enumerate(pieces) if i not in done]
for i, (start, end) in enumerate(pieces)]
try:
await self._report_until_done(tasks, artifact, progress, path, layout, started)
await self._report_until_done(tasks, artifact, progress, total, started)
except BaseException as e:
cancel.set()
if isinstance(e, DownloadCancelled):
@@ -293,32 +232,9 @@ class ModelManagerSP:
for task in tasks:
task.cancel() # drops queued pieces; running ones see the event at their next block
await asyncio.gather(*tasks, return_exceptions=True)
await loop.run_in_executor(None, pool.shutdown) # let running pieces stop so none lands after the save
_save_resume_state(path, layout, progress.snapshot()[1])
raise
_remove_download_state(path)
artifact.downloadProgress.resuming = False
del self._download_start_times[artifact.fileName]
async def _download_with_retries(self, url: str, path: str, artifact) -> None:
for attempt in range(DOWNLOAD_ATTEMPTS):
try:
await self._download_file(url, path, artifact)
return
except Exception as e:
if not _is_transient(e) or attempt == DOWNLOAD_ATTEMPTS - 1:
raise
delay = RETRY_BACKOFF * 2 ** attempt
cloudlog.warning(f"Retrying {artifact.fileName} in {delay:g}s after {type(e).__name__}: {e}")
progress = artifact.downloadProgress
self._set_progress(artifact, progress.status, progress.progress, progress.eta, 0.0) # stalled: no speed
deadline = time.monotonic() + delay
while (remaining := deadline - time.monotonic()) > 0:
if self._download_interrupted():
raise DownloadCancelled("Download cancelled") from e
await asyncio.sleep(min(RETRY_POLL, remaining))
async def _process_artifact(self, artifact, destination_path: str) -> None:
if not artifact.downloadUri.uri:
return None
@@ -330,39 +246,37 @@ class ModelManagerSP:
filename = artifact.fileName
full_path = os.path.join(destination_path, filename)
status = custom.ModelManagerSP.DownloadStatus
artifact.downloadProgress.resuming = False
artifact.downloadProgress.canceling = False
try:
if _download_in_progress(full_path):
cloudlog.info(f"Resuming interrupted download of {filename}")
artifact.downloadProgress.resuming = True
else:
self._set_progress(artifact, status.verifying, 0)
if await verify_file(full_path, expected_hash):
self._set_progress(artifact, status.cached, 100)
return
self._set_progress(artifact, status.verifying, 0)
if await verify_file(full_path, expected_hash):
self._set_progress(artifact, status.cached, 100)
return
self._set_progress(artifact, status.downloading, 0)
await self._download_with_retries(url, full_path, artifact)
await self._download_file(url, full_path, artifact)
self._set_progress(artifact, status.verifying, 99)
if not await verify_file(full_path, expected_hash):
_remove_file(full_path)
_remove_download_state(full_path)
raise ValueError(f"Hash validation failed for {filename}")
self._set_progress(artifact, status.downloaded, 100)
except DownloadCancelled: # not a failure: the request handler clears the row
except DownloadCancelled:
self._download_start_times.pop(artifact.fileName, None)
artifact.downloadProgress.status = status.failed
artifact.downloadProgress.eta = 0
artifact.downloadProgress.speed = 0
self._sync_artifact_progress(artifact)
if self.selected_bundle:
self.selected_bundle.status = status.failed
self._report_status()
raise
except Exception as e:
cloudlog.error(f"Error downloading {filename}: {str(e)}")
if os.path.isfile(full_path): # noqa: ASYNC240
os.remove(full_path)
artifact.downloadProgress.status = status.failed
artifact.downloadProgress.eta = 0
artifact.downloadProgress.speed = 0
@@ -419,8 +333,6 @@ class ModelManagerSP:
self.params.put(ACTIVE_BUNDLE_KEYS[source], model_bundle.to_dict(), block=True)
self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
except DownloadCancelled:
raise # not a failure
except Exception:
if self.selected_bundle is not None:
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
@@ -448,14 +360,11 @@ class ModelManagerSP:
self._download_ref = ref_to_download
try:
self.download(model_to_download, Paths.model_root(), source)
except DownloadCancelled:
self.selected_bundle = None
except Exception as e:
cloudlog.exception(e) # a failure stays on the row until the next request
else:
self.selected_bundle = None
cloudlog.exception(e)
finally:
self._release_download_ref()
self.selected_bundle = None
def main_thread(self) -> None:
"""Main thread for model management"""
@@ -11,13 +11,11 @@ import contextlib
import dataclasses
import hashlib
import http.server
import json
import math
import os
import random
import re
import requests
import stat
import tempfile
import threading
import time
@@ -57,8 +55,6 @@ class ServerState:
fail_ranges: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> HTTP status, every time
fail_times: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> how many more requests 503
short_once: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges whose first answer is cut short
fail_after_others: bool = False # hold each failure until every other piece has been sent
sent: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges sent in full
stall_ranges: set[tuple[int, int]] = dataclasses.field(default_factory=set)
stall_event: threading.Event | None = None
stall_released_by_event: bool | None = None
@@ -92,10 +88,6 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
state.fail_times[rng] -= 1
status = 503
if status is not None:
others = {piece_range(i) for i in range(NUM_PIECES)} - {rng}
deadline = time.monotonic() + 5
while state.fail_after_others and not others <= state.sent and time.monotonic() < deadline:
time.sleep(0.01)
self.send_response(status)
self.end_headers()
return
@@ -123,8 +115,6 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
self.wfile.write(payload[100:])
else:
self.wfile.write(payload)
if rng is not None:
state.sent.add(rng)
@contextlib.contextmanager
@@ -143,10 +133,7 @@ def threaded_server(handler):
class InlineExecutor(concurrent.futures.Executor):
def submit(self, fn, /, *args, **kwargs):
future: concurrent.futures.Future = concurrent.futures.Future()
try:
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
except BaseException as e:
future.set_exception(e)
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
return future
@@ -160,12 +147,11 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
self.addCleanup(self._tmp.cleanup)
self.dest = self._tmp.name
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100), ('RETRY_BACKOFF', 0.01)):
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100)):
self.enterContext(mock.patch.object(manager_module, name, value))
self.reported: list[float] = []
self.reported_statuses: list[int] = []
self.reported_resuming: list[bool] = []
self.manager = ModelManagerSP.__new__(ModelManagerSP)
self.params = self.manager.params = mock.MagicMock()
@@ -186,7 +172,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
self.reported.append(float(artifact.downloadProgress.progress))
status = artifact.downloadProgress.status
self.reported_statuses.append(getattr(status, 'raw', status)) # .raw: _DynamicEnum equals its int but does not hash like it
self.reported_resuming.append(bool(artifact.downloadProgress.resuming))
def make_artifact(self):
bundle = custom.ModelManagerSP.ModelBundle.new_message()
@@ -203,24 +188,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
def path(self) -> str:
return os.path.join(self.dest, FILE_NAME)
@property
def sidecar(self) -> str:
return manager_module._sidecar_path(self.path)
def resume_state(self) -> dict:
with open(self.sidecar) as f:
return json.load(f)
def write_resume_state(self, done: list[int], total: int = len(WHOLE_BODY), file_size: int | None = None,
file_sha256: str = sha256(WHOLE_BODY)) -> None:
with open(self.path, 'wb') as f:
f.truncate(len(WHOLE_BODY) if file_size is None else file_size)
for i in done:
f.seek(i * PIECE_SIZE)
f.write(WHOLE_BODY[i * PIECE_SIZE:(i + 1) * PIECE_SIZE])
with open(self.sidecar, 'w') as f:
json.dump({"total": total, "piece_size": PIECE_SIZE, "ranged": True, "sha256": file_sha256, "done": done}, f)
def piece_requests(self) -> list[tuple[int, int]]:
return [r for r in self.server.request_ranges if r is not None and r != (0, 0)]
@@ -242,17 +209,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
def download_bundle(self, source: str):
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, source))
def request_download(self) -> dict:
self.make_artifact()
self._bundle.ref = "ref"
self._bundle.minimumSelectorVersion = helpers.REQUIRED_JSON_VERSION
self.manager.source_models = {"qcom": [self._bundle], "chestnut": []}
self.manager.params, store = self._make_params_with_store()
store["ModelManager_DownloadRef"] = "ref"
with mock.patch.object(manager_module.Paths, 'model_root', return_value=self.dest):
self.manager._process_download_requests()
return store
def read_path(self) -> bytes:
with open(self.path, 'rb') as f:
return f.read()
@@ -269,7 +225,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
params.get.side_effect = get
params.put.side_effect = put
params.remove.side_effect = lambda key: put(key, None)
return params, store
@@ -283,7 +238,6 @@ class TestManagerDownload(ManagerDownloadTestBase):
assert max(self.reported) <= 99.0, "progress holds below 100 until the hash check"
assert artifact.fileName not in self.manager._download_start_times
assert artifact.downloadProgress.speed > 0, "a running download shows its speed"
assert not os.path.exists(self.sidecar), "the resume state goes once every piece has landed"
def test_speed_counts_bytes_landed_before_the_first_report(self):
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
@@ -342,42 +296,9 @@ class TestManagerDownload(ManagerDownloadTestBase):
def test_permanent_failure_is_not_retried(self):
self.server.fail_ranges = {piece_range(1): 404}
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
with self.assertRaises(requests.exceptions.HTTPError):
self.process_artifact()
self.download_file()
assert self.piece_requests().count(piece_range(1)) == 1
assert os.path.isfile(self.path)
assert self.resume_state()["done"] == [i for i in range(NUM_PIECES) if i != 1]
def test_failed_transfer_resumes_on_retry(self):
self.server.fail_times = {piece_range(1): manager_module.PIECE_ATTEMPTS} # the first transfer gives up on piece 1
self.server.fail_after_others = True # by then every other piece is saved
self.process_artifact()
assert self.read_path() == WHOLE_BODY
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS + 1, "the retry fetched piece 1"
others = [i for i in range(NUM_PIECES) if i != 1]
assert all(self.piece_requests().count(piece_range(i)) == 1 for i in others), "the retry skipped every saved piece"
def test_lasting_transient_failure_gives_up(self):
self.server.fail_ranges = {piece_range(1): 503}
with self.assertRaises(requests.exceptions.HTTPError):
self.process_artifact()
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS * manager_module.DOWNLOAD_ATTEMPTS
def test_cancel_ends_retry_backoff(self):
self.enterContext(mock.patch.object(manager_module, 'RETRY_BACKOFF', 30.0))
self.manager._download_ref = b"ref"
def fail_then_cancel(*args):
self.params.get.return_value = None # the user cancels while the manager waits to retry
raise requests.exceptions.ConnectionError("dropped")
transfer = self.enterContext(mock.patch.object(self.manager, '_download_file', side_effect=fail_then_cancel))
started = time.monotonic()
with self.assertRaises(DownloadCancelled):
self.process_artifact()
assert time.monotonic() - started < 5, "the wait must end at the cancel, not after 30 s"
assert transfer.call_count == 1, "no second transfer after a cancel"
assert self.artifact.downloadProgress.status != custom.ModelManagerSP.DownloadStatus.failed
def test_removing_download_ref_cancels(self):
stall_event = self.stall(3)
@@ -395,51 +316,6 @@ class TestManagerDownload(ManagerDownloadTestBase):
assert self.server.stall_released_by_event is True, "the cancel must have caught the piece mid-transfer"
end = piece_range(3)[1] + 1
assert self.read_path()[end - 100:end] == bytes(100), "the cancelled piece must stop before its last bytes land"
done = self.resume_state()["done"]
assert done and 3 not in done, "a cancel records the finished pieces, and only those, for resume"
def test_resume_skips_complete_pieces(self):
self.write_resume_state(done=[0, 4])
self.download_file()
assert piece_range(0) not in self.piece_requests() and piece_range(4) not in self.piece_requests()
assert self.read_path() == WHOLE_BODY
assert min(self.reported) >= 2 * PIECE_SIZE / len(WHOLE_BODY) * 100 - 1, "progress must not restart below the resumed share"
def test_unusable_resume_state_starts_over(self):
def malformed(content: bytes):
self.write_resume_state(done=[0])
with open(self.sidecar, 'wb') as f:
f.write(content)
layout = {"total": len(WHOLE_BODY), "piece_size": PIECE_SIZE, "ranged": True, "sha256": sha256(WHOLE_BODY)}
cases = {
"other size": lambda: self.write_resume_state(done=[0], total=len(WHOLE_BODY) + 1),
"other bytes": lambda: self.write_resume_state(done=[0], file_sha256="0" * 64),
"truncated file": lambda: self.write_resume_state(done=[0], file_size=PIECE_SIZE),
"not an object": lambda: malformed(b'[]'),
"cut-off json": lambda: malformed(b'{"total": '),
"done not a list": lambda: malformed(json.dumps({**layout, "done": 3}).encode()),
"done not ints": lambda: malformed(json.dumps({**layout, "done": ["0", None]}).encode()),
}
for name, write_state in cases.items():
with self.subTest(name):
self.server.request_ranges = []
write_state()
self.download_file()
assert sorted(self.piece_requests()) == [piece_range(i) for i in range(NUM_PIECES)], "every piece is fetched again"
assert self.read_path() == WHOLE_BODY
def test_pieces_and_resume_state_are_synced_to_disk(self):
real_fsync, synced = os.fsync, []
def record_sync(fd):
st = os.fstat(fd)
synced.append("dir" if stat.S_ISDIR(st.st_mode) else "model" if os.path.exists(self.path) and st.st_ino == os.stat(self.path).st_ino else "sidecar")
real_fsync(fd)
with mock.patch.object(manager_module.os, 'fsync', side_effect=record_sync):
self.download_file()
assert synced.count("model") == NUM_PIECES, f"expected one fsync per piece, got {synced.count('model')}"
assert "sidecar" in synced and "dir" in synced, "sidecar file and directory must be fsync'd"
def test_replaced_download_ref_queues_instead_of_canceling(self):
self.params.get.return_value = b"other-ref"
@@ -469,26 +345,9 @@ class TestProcessArtifact(ManagerDownloadTestBase):
self.server.corrupt = True
with self.assertRaises(ValueError):
self.process_artifact()
assert not os.path.isfile(self.path) and not os.path.exists(self.sidecar), "a corrupt file must not be resumed from"
assert not os.path.isfile(self.path)
assert self.artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.failed
def test_interrupted_download_resumes_without_reverifying(self):
self.write_resume_state(done=[0, 1, 2])
artifact = self.process_artifact()
ds = custom.ModelManagerSP.DownloadStatus
assert self.reported_statuses[0] == ds.downloading, "an in-progress download is not verified first"
assert piece_range(0) not in self.piece_requests()
assert artifact.downloadProgress.status == ds.downloaded
assert any(self.reported_resuming), "a resumed download is flagged resuming until its first progress report"
assert not artifact.downloadProgress.resuming
def test_resume_with_every_piece_done_clears_resuming(self):
self.write_resume_state(done=list(range(NUM_PIECES)))
artifact = self.process_artifact()
assert self.piece_requests() == []
assert artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.downloaded
assert not artifact.downloadProgress.resuming
def test_probe_failure_moves_past_verifying(self):
self.server.fail_ranges = {(0, 0): 404} # the size probe
with self.assertRaises(requests.exceptions.HTTPError):
@@ -515,23 +374,6 @@ class TestProcessArtifact(ManagerDownloadTestBase):
self.download_bundle("qcom")
assert ACTIVE_BUNDLE_KEYS["qcom"] not in store
assert os.path.isfile(self.path), "cancel must not delete a cached model"
assert self.manager.selected_bundle.status != custom.ModelManagerSP.DownloadStatus.failed, "a cancel is not a failure"
def test_failed_download_stays_on_the_row(self):
self.server.fail_ranges = {piece_range(1): 404}
store = self.request_download()
selected = self.manager.selected_bundle
assert selected is not None and selected.status == custom.ModelManagerSP.DownloadStatus.failed, "a failure stays on the row"
assert store["ModelManager_DownloadRef"] is None, "the failed request is released"
def test_finished_download_clears_the_row(self):
self.request_download()
assert self.manager.selected_bundle is None
def test_cancelled_download_clears_the_row(self):
self.pm.send.side_effect = lambda *args: self.manager.params.remove("ModelManager_DownloadRef") # cancel at the first report
self.request_download()
assert self.manager.selected_bundle is None
def test_download_ref_is_released_only_if_unchanged(self):
for current, removed in ((b"new-ref", False), (b"ref", True)):
@@ -1,67 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import numpy as np
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
ACCEL_BOOST_MIN_SPEED = 1.0
ACCEL_BOOST_MAX = (0.4, 0.25, 0.1) # aggressive, standard, relaxed, m/s^2
ACCEL_BOOST_BUILD, ACCEL_BOOST_RELEASE = 0.2, 1.0 # m/s^3
BRAKE_PROB = (0.1, 0.3) # the model's brake press in 4 s: it wants to slow
GAS_PROB = (0.6, 0.8) # predicts a human gas press at this hypothesis at about 23%
CURVE_LAT_ACCEL = (1.5, 2.5)
def get_boost_max(personality=log.LongitudinalPersonality.standard):
if personality == log.LongitudinalPersonality.relaxed:
return ACCEL_BOOST_MAX[2]
elif personality == log.LongitudinalPersonality.standard:
return ACCEL_BOOST_MAX[1]
elif personality == log.LongitudinalPersonality.aggressive:
return ACCEL_BOOST_MAX[0]
else:
raise NotImplementedError("Longitudinal personality not supported")
class AccelBoostSP:
def __init__(self):
self.params = Params()
self.frame = -1
self.enabled = False
self.boost = 0.0
self.total_boost = 0.0
def update_params(self):
self.frame += 1
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) != 0:
return False
enabled = self.params.get_bool("AccelBoostSP")
if enabled == self.enabled:
return False
self.enabled = enabled
self.boost = self.total_boost = 0.0
return True
def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
model = sm['modelV2']
brake_probs = model.meta.disengagePredictions.brakePressProbs
gas_probs = model.meta.disengagePredictions.gasPressProbs
if not sm['selfdriveState'].enabled or len(brake_probs) < 2 or len(gas_probs) < 2:
self.boost = self.total_boost = 0.0
return output_a_target_e2e
want = max(float(np.interp(brake_probs[1], BRAKE_PROB, [1.0, 0.0])), float(np.interp(gas_probs[1], GAS_PROB, [0.0, 1.0])))
want = 0.0 if model.action.shouldStop else want
want *= float(np.interp(abs(model.action.desiredCurvature) * sm['carState'].vEgo ** 2, CURVE_LAT_ACCEL, [1.0, 0.0]))
target = want * get_boost_max(sm['selfdriveState'].personality)
self.boost = float(np.clip(target, self.boost - ACCEL_BOOST_RELEASE * DT_MDL, self.boost + ACCEL_BOOST_BUILD * DT_MDL))
speed_scale = np.interp(sm['carState'].vEgo, [ACCEL_BOOST_MIN_SPEED, 2 * ACCEL_BOOST_MIN_SPEED], [0.0, 1.0])
self.total_boost = self.boost * speed_scale * np.interp(output_a_target_e2e, [-0.7, -0.2, 5.0], [0.0, 1.0, 1.0], right=0.0)
return output_a_target_e2e + self.total_boost
@@ -9,8 +9,6 @@ from openpilot.cereal import messaging, custom
from opendbc.car import structs
from openpilot.common.constants import CV
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
@@ -31,8 +29,6 @@ class LongitudinalPlannerSP:
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
self.source = LongitudinalPlanSource.cruise
self.e2e_alerts_helper = E2EAlertsHelper()
self.accel_boost_sp = AccelBoostSP()
self.openpilot_longitudinal = CP.openpilotLongitudinalControl
self.output_v_target = 0.
self.output_a_target = 0.
@@ -68,9 +64,6 @@ class LongitudinalPlannerSP:
return self.output_v_target, self.output_a_target
def update(self, sm: messaging.SubMaster) -> None:
if self.accel_boost_sp.update_params():
self.accel_boost = self.accel_boost_sp if self.accel_boost_sp.enabled and self.openpilot_longitudinal else AccelBoost()
self.events_sp.clear()
self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -1,81 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from types import SimpleNamespace
from openpilot.common.parameterized import parameterized
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP, ACCEL_BOOST_MAX
def make_sm(brake_prob=0.05, gas_prob=0.3, lat_accel=0.0, v_ego=30.0, personality=0, enabled=True, should_stop=False):
meta = SimpleNamespace(disengagePredictions=SimpleNamespace(brakePressProbs=[0.0, brake_prob, 0.0, 0.0, 0.0, 0.0],
gasPressProbs=[0.0, gas_prob, 0.0, 0.0, 0.0, 0.0]))
action = SimpleNamespace(shouldStop=should_stop, desiredCurvature=lat_accel / v_ego ** 2)
return {'modelV2': SimpleNamespace(meta=meta, action=action), 'carState': SimpleNamespace(vEgo=v_ego),
'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality)}
def settle(sm, frames=100):
boost = AccelBoostSP()
for _ in range(frames):
boost.update(sm, 0.0, 9.0, 9.0)
return boost.total_boost
class TestAccelBoostSP(OpenpilotTestCase):
@parameterized.expand([(0,), (1,), (2,)])
def test_personality_max(self, personality):
assert abs(settle(make_sm(personality=personality)) - ACCEL_BOOST_MAX[personality]) < 1e-6
def test_brake_gate_closes(self):
assert settle(make_sm(brake_prob=0.5)) == 0.0
def test_gas_reopens(self):
assert abs(settle(make_sm(brake_prob=0.5, gas_prob=0.9)) - ACCEL_BOOST_MAX[0]) < 1e-6
def test_should_stop(self):
assert settle(make_sm(gas_prob=0.9, should_stop=True)) == 0.0
@parameterized.expand([(1.5, 1.0), (2.0, 0.5), (2.5, 0.0), (-2.0, 0.5)])
def test_curve_fade(self, lat_accel, scale):
assert abs(settle(make_sm(lat_accel=lat_accel)) - scale * ACCEL_BOOST_MAX[0]) < 1e-6
def test_low_speed(self):
assert settle(make_sm(v_ego=0.5)) == 0.0
def test_disabled_resets(self):
boost = AccelBoostSP()
for _ in range(100):
boost.update(make_sm(), 0.0, 9.0, 9.0)
assert boost.update(make_sm(enabled=False), 0.0, 9.0, 9.0) == 0.0
assert boost.boost == 0.0 and boost.total_boost == 0.0
def test_build_and_release_rates(self):
boost = AccelBoostSP()
boost.update(make_sm(), 0.0, 9.0, 9.0)
assert abs(boost.total_boost - 0.01) < 1e-6
for _ in range(100):
boost.update(make_sm(), 0.0, 9.0, 9.0)
boost.update(make_sm(brake_prob=0.5), 0.0, 9.0, 9.0)
assert abs(boost.total_boost - (ACCEL_BOOST_MAX[0] - 0.05)) < 1e-6
def test_param_switch(self):
params = Params()
boost = AccelBoostSP()
params.put_bool("AccelBoostSP", True, block=True)
assert boost.update_params() and boost.enabled
for _ in range(100):
boost.update(make_sm(), 0.0, 9.0, 9.0)
params.put_bool("AccelBoostSP", False, block=True)
for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL) - 1):
assert not boost.update_params()
assert boost.update_params() and not boost.enabled
assert boost.boost == 0.0 and boost.total_boost == 0.0
@@ -0,0 +1,3 @@
params_learner
paramsd
locationd
@@ -0,0 +1,30 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
live_ekf = env.RednoseCompileFilter(
target='live',
filter_gen_script='models/live_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['live_kf_constants.h'],
gen_script_deps=rednose_gen_deps,
)
# locationd build
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
lenv.Depends(locationd, rednose)
lenv.Depends(locationd, live_ekf)
@@ -1,181 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import time
from collections import deque
import numpy as np
import openpilot.cereal.messaging as messaging
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.common.transformations.orientation import rot_from_euler
EARTH_RADIUS = 6378137.0
GPS_LATENCY = {'gpsLocation': 0.630, 'gpsLocationExternal': 0.095} # s, empirical fix latency per receiver
RESET_DISTANCE = 100.0 # m
MAX_REJECTS = 5
MAX_POS_STD = 50.0 # m
MIN_BEARING_SPEED = 5.0 # m/s
MAX_BEARING_ACCURACY = 10.0 # deg
GPS_TIMEOUT = 2.0 # s
SEED_INTERVAL = 60.0 # s
def wrap(a: float) -> float:
return (a + math.pi) % (2 * math.pi) - math.pi
class GpsFuser:
def __init__(self):
self.t: float | None = None
self.lat = self.lon = self.heading = math.nan
self.pos_var = self.heading_var = math.inf
self.history: deque = deque(maxlen=40)
self.rejects = 0
@property
def initialized(self) -> bool:
return math.isfinite(self.lat)
@property
def valid(self) -> bool:
return self.initialized and math.isfinite(self.heading) and self.pos_var < MAX_POS_STD ** 2
def _move(self, north: float, east: float) -> None:
self.lat += math.degrees(north / EARTH_RADIUS)
self.lon += math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(self.lat))))
@staticmethod
def _offset(lat: float, lon: float, ref_lat: float, ref_lon: float) -> tuple[float, float]:
north = math.radians(lat - ref_lat) * EARTH_RADIUS
east = math.radians(lon - ref_lon) * EARTH_RADIUS * math.cos(math.radians(ref_lat))
return north, east
def _state_at(self, t: float) -> tuple[float, float, float]:
for ht, lat, lon, heading in reversed(self.history):
if ht <= t:
return lat, lon, heading
return self.lat, self.lon, self.heading
def predict(self, t: float, speed: float, yaw_rate: float) -> None:
dt = 0.0 if self.t is None else t - self.t
self.t = t
if not 0.0 < dt < 1.0 or not self.initialized:
return
if math.isfinite(self.heading):
self.heading = (self.heading + yaw_rate * dt) % (2 * math.pi)
self._move(speed * dt * math.cos(self.heading), speed * dt * math.sin(self.heading))
# dead reckoning error grows with distance driven (speed scale and heading error), not as a random walk
drift = (0.03 + math.sqrt(min(self.heading_var, 1.0))) * abs(speed) * dt
self.pos_var = (math.sqrt(self.pos_var) + drift) ** 2 + 0.01 * dt
self.heading_var += 1e-5 * dt
self.history.append((t, self.lat, self.lon, self.heading))
def update_gps(self, t: float, gps, latency: float) -> None:
# qcom reports no horizontal accuracy
accuracy = gps.horizontalAccuracy if gps.horizontalAccuracy > 0 else math.sqrt(3) * gps.verticalAccuracy
if not gps.hasFix or not 0 < accuracy < 1500 or gps.speedAccuracy <= 0:
return
if abs(gps.latitude) > 90 or abs(gps.longitude) > 180:
return
then_lat, then_lon, then_heading = self._state_at(t - latency)
moved = self._offset(self.lat, self.lon, then_lat, then_lon) if self.initialized else (0.0, 0.0)
bearing = math.radians(gps.bearingDeg)
bearing_ok = gps.speed > MIN_BEARING_SPEED and 0 < gps.bearingAccuracyDeg < MAX_BEARING_ACCURACY
pos_r = accuracy ** 2
innov = self._offset(gps.latitude, gps.longitude, then_lat, then_lon) if self.initialized else (math.inf, math.inf)
dist = math.hypot(*innov)
if not self.initialized or dist > RESET_DISTANCE or self.rejects >= MAX_REJECTS:
self.lat, self.lon = gps.latitude, gps.longitude
self._move(*moved)
self.pos_var = pos_r
self.rejects = 0
self.history.clear()
if bearing_ok:
self.heading, self.heading_var = bearing, math.radians(gps.bearingAccuracyDeg) ** 2
return
s = self.pos_var + pos_r
if dist ** 2 > 9.0 * s and dist > 10.0:
self.rejects += 1
return
self.rejects = 0
k = self.pos_var / s
lat0, lon0 = self.lat, self.lon
self._move(k * innov[0], k * innov[1])
dlat, dlon = self.lat - lat0, self.lon - lon0
self.history = deque(((ht, lat + dlat, lon + dlon, h) for ht, lat, lon, h in self.history), maxlen=self.history.maxlen)
self.pos_var *= 1 - k
if bearing_ok:
head_r = math.radians(gps.bearingAccuracyDeg) ** 2
if not math.isfinite(self.heading):
self.heading, self.heading_var = bearing, head_r
else:
kh = self.heading_var / (self.heading_var + head_r)
self.heading = (self.heading + kh * wrap(bearing - then_heading)) % (2 * math.pi)
self.heading_var *= 1 - kh
def main():
params = Params()
gps_service = get_gps_location_service(params)
latency = GPS_LATENCY[gps_service]
sm = messaging.SubMaster(['deviceMotion', 'carState', 'extrinsicsCalibration', gps_service], poll='deviceMotion')
pm = messaging.PubMaster(['gpsLocationSP'])
fuser = GpsFuser()
calib_from_device = np.eye(3)
last_seed = 0.0
while True:
sm.update()
t = sm.logMonoTime['deviceMotion'] * 1e-9
rpy = list(sm['extrinsicsCalibration'].rpyCalib)
if sm.updated['extrinsicsCalibration'] and len(rpy) == 3:
calib_from_device = rot_from_euler(rpy).T
gps = sm[gps_service]
gps_t = sm.logMonoTime[gps_service] * 1e-9
if sm.updated[gps_service] and sm.valid[gps_service]:
fuser.update_gps(gps_t, gps, latency)
if sm.valid['deviceMotion']:
w = sm['deviceMotion'].angularVelocityDevice
yaw_rate = float((calib_from_device @ np.array([w.x, w.y, w.z]))[2])
fuser.predict(t, sm['carState'].vEgo, yaw_rate)
gps_ok = gps.hasFix and 0 <= t - gps_t < GPS_TIMEOUT
msg = messaging.new_message('gpsLocationSP', valid=fuser.valid or gps_ok)
out = msg.gpsLocationSP
out.gpsOK = gps_ok
out.fused = fuser.valid
if fuser.valid:
out.hasFix = True
out.latitude, out.longitude = fuser.lat, fuser.lon
out.bearingDeg = math.degrees(fuser.heading) % 360
out.bearingAccuracyDeg = math.degrees(math.sqrt(fuser.heading_var))
out.horizontalAccuracy = math.sqrt(fuser.pos_var)
out.speed = sm['carState'].vEgo
elif gps_ok:
out.hasFix = True
out.latitude, out.longitude = gps.latitude, gps.longitude
out.bearingDeg, out.bearingAccuracyDeg = gps.bearingDeg, gps.bearingAccuracyDeg
out.horizontalAccuracy, out.speed = gps.horizontalAccuracy, gps.speed
pm.send('gpsLocationSP', msg)
if fuser.valid and time.monotonic() - last_seed > SEED_INTERVAL:
params.put("LastGPSPositionFused", json.dumps({"latitude": fuser.lat, "longitude": fuser.lon}))
last_seed = time.monotonic()
if __name__ == "__main__":
main()
@@ -0,0 +1,750 @@
#include "sunnypilot/selfdrive/locationd/locationd.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
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 VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
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";
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
res[i] = floatlist[i];
}
return res;
}
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_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
meas.setValue(kj::arrayPtr(val.data(), val.size()));
meas.setStd(kj::arrayPtr(std.data(), std.size()));
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<LiveKalman>();
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_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_location(cereal::LiveLocationKalman::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_LEN>(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_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
VectorXd vel_device = device_from_ecef * vel_ecef;
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(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<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(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 vel_calib = this->calib_from_device * vel_device;
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, 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 calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
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_LEN>(STATE_ACCELERATION_START);
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
Vector3d nans = Vector3d(NAN, NAN, NAN);
// TODO fill in NED and Calibrated stds
// write measurements to msg
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
init_measurement(fix.initFilterState(), predicted_state, predicted_std, 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));
fix.setDeviceStable(!this->device_fell);
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
this->device_fell = false;
//fix.setGpsWeek(this->time.week);
//fix.setGpsTimeOfWeek(this->time.tow);
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
fix.setTimeSinceReset(time_since_reset);
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
} else {
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
}
}
VectorXd Localizer::get_position_geodetic() {
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(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.which() == cereal::SensorEventData::GYRO_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_LEN>(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.which() == cereal::SensorEventData::ACCELERATION) {
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 |= (floatlist2vector(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_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(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 = (floatlist2vector(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<LocalCoord>(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_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(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_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(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 * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(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 = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(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::ExtrinsicsCalibration::Reader& log) {
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if (log.getRpyCalib().size() > 0) {
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["extrinsicsCalibration"] += 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::ExtrinsicsCalibration::Status::CALIBRATED;
this->observation_values_invalid["extrinsicsCalibration"] *= 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_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(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<cereal::Event>();
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.isExtrinsicsCalibration()) {
this->handle_live_calib(t, log.getExtrinsicsCalibration());
}
this->finite_check();
this->update_reset_tracker();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
this->build_live_location(liveLoc);
liveLoc.setSensorsOK(sensorsOK);
liveLoc.setGpsOK(gpsOK);
liveLoc.setInputsOK(inputsOK);
return msg_builder.toBytes();
}
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<std::string, double> &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_LEN, STATE_ECEF_POS_ERR_LEN>(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<const char *> service_list = {gps_location_socket, "cameraOdometry", "extrinsicsCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "extrinsicsCalibration", "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 msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("liveLocationKalman", bytes.begin(), 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("LastGPSPositionLLK", lastGPSPosJSON);
}
cnt++;
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
}
@@ -0,0 +1,100 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include <deque>
#include <fstream>
#include <memory>
#include <map>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "sunnypilot/common/transformations/coordinates.hpp"
#include "sunnypilot/common/transformations/orientation.hpp"
#include "sunnypilot/system/sensord/sensors/constants.h"
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
#define VISION_DECIMATION 2
#define SENSOR_DECIMATION 10
#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<std::string, double> &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();
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void build_live_location(cereal::LiveLocationKalman::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::ExtrinsicsCalibration::Reader& log);
void input_fake_gps_observations(double current_time);
private:
std::unique_ptr<LiveKalman> 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<double> posenet_stds;
std::unique_ptr<LocalCoord> converter;
int64_t unix_timestamp_millis = 0;
double reset_tracker = 0.0;
bool device_fell = false;
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<std::string, double> 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);
};
@@ -0,0 +1 @@
generated/
+180
View File
@@ -0,0 +1,180 @@
#!/usr/bin/env python3
import math
import sys
from typing import Any
import numpy as np
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
from openpilot.common.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
i = 0
def _slice(n):
global i
s = slice(i, i + n)
i += n
return s
class States:
# Vehicle model params
STIFFNESS = _slice(1) # [-]
STEER_RATIO = _slice(1) # [-]
ANGLE_OFFSET = _slice(1) # [rad]
ANGLE_OFFSET_FAST = _slice(1) # [rad]
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
class CarKalman(KalmanFilter):
name = 'car'
initial_x = np.array([
1.0,
15.0,
0.0,
0.0,
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
Q = np.diag([
(.05 / 100)**2,
.01**2,
math.radians(0.02)**2,
math.radians(0.25)**2,
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
math.radians(1)**2,
])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
global_vars = [
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
@staticmethod
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
# Linearized single-track lateral dynamics, equations 7.211-7.213
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
# globals
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":
generated_dir = sys.argv[2]
CarKalman.generate_code(generated_dir)
@@ -0,0 +1,92 @@
import os
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]
@@ -0,0 +1,122 @@
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> 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<EKFSym>(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<int>(),
std::vector<int>{3}, std::vector<std::string>(), 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<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> 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<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}
@@ -0,0 +1,66 @@
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#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<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &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<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> 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<EKFSym> 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<int, MatrixXdr> obs_noise;
};
+242
View File
@@ -0,0 +1,242 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from openpilot.sunnypilot.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 <unordered_map>\n"
live_kf_header += "#include <eigen3/Eigen/Dense>\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<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> 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)
@@ -0,0 +1 @@
out/
@@ -1,73 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import math
from types import SimpleNamespace
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.selfdrive.locationd.gps_fuser import EARTH_RADIUS, GPS_LATENCY, GpsFuser
LAT0, LON0 = 39.0, -77.0
LATENCY = GPS_LATENCY['gpsLocation']
def fix(north, east, speed=30.0, bearing=90.0, h_acc=0.0, v_acc=1.5):
return SimpleNamespace(latitude=LAT0 + math.degrees(north / EARTH_RADIUS),
longitude=LON0 + math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(LAT0)))),
speed=speed, bearingDeg=bearing, horizontalAccuracy=h_acc, verticalAccuracy=v_acc,
speedAccuracy=0.3, bearingAccuracyDeg=0.5, hasFix=True)
def position(f: GpsFuser) -> tuple[float, float]:
north = math.radians(f.lat - LAT0) * EARTH_RADIUS
east = math.radians(f.lon - LON0) * EARTH_RADIUS * math.cos(math.radians(LAT0))
return north, east
class TestGpsFuser(OpenpilotTestCase):
def drive(self, f, seconds, speed=30.0, start=0.0, gps=lambda t, true_east: fix(0.0, true_east)):
t = start
while t < start + seconds:
t = round(t + 0.05, 2)
f.predict(t, speed, 0.0)
if abs(t - round(t)) < 1e-6:
f.update_gps(t, gps(t, speed * (t - LATENCY)), LATENCY)
return t
def test_compensates_latency(self):
f = GpsFuser()
t = self.drive(f, 30)
self.assertTrue(f.valid)
north, east = position(f)
self.assertAlmostEqual(east, 30.0 * t, delta=3.0)
self.assertAlmostEqual(north, 0.0, delta=1.0)
def test_rejects_single_jump(self):
f = GpsFuser()
t = self.drive(f, 20)
f.predict(t + 0.05, 30.0, 0.0)
f.update_gps(t + 0.05, fix(40.0, 30.0 * (t + 0.05 - LATENCY)), LATENCY)
self.assertAlmostEqual(position(f)[0], 0.0, delta=2.0)
def test_follows_persistent_offset(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 10, start=t, gps=lambda _, e: fix(150.0, e))
self.assertAlmostEqual(position(f)[0], 150.0, delta=3.0)
def test_ignores_bearing_when_slow(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 5, speed=1.0, start=t, gps=lambda _, e: fix(0.0, e, speed=1.0, bearing=270.0))
self.assertAlmostEqual(math.degrees(f.heading), 90.0, delta=1.0)
def test_invalid_after_long_outage(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 120, start=t, gps=lambda _, e: SimpleNamespace(hasFix=False, horizontalAccuracy=0.0, verticalAccuracy=0.0,
speedAccuracy=0.0, latitude=0.0, longitude=0.0))
self.assertFalse(f.valid)
@@ -0,0 +1,96 @@
import platform
import unittest
import json
import random
import time
import capnp
import openpilot.cereal.messaging as messaging
from openpilot.cereal.services import SERVICE_LIST
from openpilot.common.params import Params
from openpilot.common.transformations.coordinates import ecef2geodetic
from openpilot.system.manager.process_config import managed_processes
from openpilot.common.test import OpenpilotTestCase
@unittest.skipIf(platform.system() == 'Darwin', "msgq unsupported on macOS")
class TestLocationdProc(OpenpilotTestCase):
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'extrinsicsCalibration',
'accelerometer', 'gyroscope']
def setup_method(self):
self.pm = messaging.PubMaster(self.LLD_MSGS)
self.params = Params()
self.params.put_bool("UbloxAvailable", True)
managed_processes['locationd_llk'].start()
def teardown_method(self):
managed_processes['locationd_llk'].stop()
def get_msg(self, name, t):
try:
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal":
msg.gpsLocationExternal.flags = 1
msg.gpsLocationExternal.hasFix = True
msg.gpsLocationExternal.verticalAccuracy = 1.0
msg.gpsLocationExternal.speedAccuracy = 1.0
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
msg.gpsLocationExternal.latitude = float(self.lat)
msg.gpsLocationExternal.longitude = float(self.lon)
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = float(self.alt)
#if name == "gnssMeasurements":
# msg.gnssMeasurements.measTime = t
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
# msg.gnssMeasurements.positionECEF.valid = True
# msg.gnssMeasurements.velocityECEF.value = []
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
# msg.gnssMeasurements.velocityECEF.valid = True
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):
random.seed(123489234)
self.params.remove('LastGPSPositionLLK')
self.x = -2710700 + (random.random() * 1e5)
self.y = -4280600 + (random.random() * 1e5)
self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
# get fake messages at the correct frequency, listed in services.py
msgs = []
for sec in range(65):
for name in self.LLD_MSGS:
for j in range(int(SERVICE_LIST[name].frequency)):
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
self.pm.send(msg.which(), msg)
if msg.which() == "cameraOdometry":
self.pm.wait_for_readers_to_update(msg.which(), timeout=1, dt=0.005)
for _ in range(50):
val = self.params.get('LastGPSPositionLLK')
if val is not None:
break
time.sleep(0.1)
self.assertIsNotNone(val, "LastGPSPositionLLK not written within 5s")
lastGPS = json.loads(val)
self.assertAlmostEqual(lastGPS['latitude'], self.lat, delta=0.001)
self.assertAlmostEqual(lastGPS['longitude'], self.lon, delta=0.001)
self.assertAlmostEqual(lastGPS['altitude'], self.alt, delta=0.001)
@@ -620,19 +620,6 @@
}
]
},
{
"key": "AccelBoostSP",
"widget": "toggle",
"title": "Acceleration Boost",
"description": "Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first.",
"enablement": [
{
"type": "capability",
"field": "has_longitudinal_control",
"equals": true
}
]
},
{
"key": "IntelligentCruiseButtonManagement",
"widget": "toggle",
@@ -1362,17 +1349,7 @@
"key": "TorqueBar",
"widget": "toggle",
"title": "Steering Arc",
"description": "Display steering arc on the driving screen when lateral control is enabled.",
"visibility": [
{
"type": "not",
"condition": {
"type": "capability",
"field": "device_type",
"equals": "mici"
}
}
]
"description": "Display steering arc on the driving screen when lateral control is enabled."
},
{
"key": "ShowTurnSignals",
@@ -1412,6 +1389,12 @@
}
]
},
{
"key": "TripSummary",
"widget": "toggle",
"title": "Trip Summary",
"description": "Show a summary of distance and engagement when you shift into park after a drive."
},
{
"key": "RocketFuel",
"widget": "toggle",
@@ -30,12 +30,6 @@ sections:
label: Relaxed
enablement:
- $ref: '#/macros/longitudinal'
- key: AccelBoostSP
widget: toggle
title: Acceleration Boost
description: "Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first."
enablement:
- $ref: '#/macros/longitudinal'
- key: IntelligentCruiseButtonManagement
widget: toggle
title: Intelligent Cruise Button Management (ICBM) (Alpha)
@@ -20,8 +20,6 @@ sections:
widget: toggle
title: Steering Arc
description: Display steering arc on the driving screen when lateral control is enabled.
visibility:
- $ref: '#/macros/hide_on_mici'
- key: ShowTurnSignals
widget: toggle
title: Display Turn Signals
@@ -39,6 +37,10 @@ sections:
description: Show a timer on the HUD when the car is at a standstill.
visibility:
- $ref: '#/macros/hide_on_mici'
- key: TripSummary
widget: toggle
title: Trip Summary
description: Show a summary of distance and engagement when you shift into park after a drive.
- key: RocketFuel
widget: toggle
title: Real-time Acceleration Bar
+13 -16
View File
@@ -19,9 +19,10 @@ from openpilot.tools.lib.route import Route
REPO_ID = os.environ.get("SUNNYPILOT_CAR_SEGMENTS_REPO_ID", "sunnypilot/sunnypilotCarSegments")
def upload_route(route_name: str, dry_run: bool = False, local_dir: str | None = None) -> None:
log_paths = Route(route_name, data_dir=local_dir).log_paths()
valid_segments = [(i, path) for i, path in enumerate(log_paths) if path is not None]
def upload_route(route_name: str, dry_run: bool = False) -> None:
route = Route(route_name)
log_paths = route.log_paths()
valid_segments = [(i, url) for i, url in enumerate(log_paths) if url is not None]
print(f"Route: {route_name}")
print(f"Segments: {len(valid_segments)}/{len(log_paths)}")
@@ -33,17 +34,14 @@ def upload_route(route_name: str, dry_run: bool = False, local_dir: str | None =
api = HfApi()
with tempfile.TemporaryDirectory() as tmpdir:
for seg_idx, src in tqdm(valid_segments, desc="Uploading"):
filename = src.split("?")[0].rsplit("/", 1)[-1]
if local_dir is not None:
local_path = src
else:
local_path = os.path.join(tmpdir, f"{seg_idx}_{filename}")
resp = requests.get(src, stream=True)
resp.raise_for_status()
with open(local_path, "wb") as f:
for chunk in resp.iter_content(chunk_size=8192):
f.write(chunk)
for seg_idx, url in tqdm(valid_segments, desc="Uploading"):
filename = url.split("?")[0].rsplit("/", 1)[-1]
local_path = os.path.join(tmpdir, f"{seg_idx}_{filename}")
resp = requests.get(url, stream=True)
resp.raise_for_status()
with open(local_path, "wb") as f:
for chunk in resp.iter_content(chunk_size=8192):
f.write(chunk)
repo_path = f"segments/{route_name.replace('|', '/')}/{seg_idx}/{filename}"
@@ -65,7 +63,6 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Upload route rlogs to sunnypilot HuggingFace dataset")
parser.add_argument("route", help="Route ID (e.g. 5beb9b58bd12b691/0000010a--a51155e496)")
parser.add_argument("--dry-run", action="store_true", help="Download and show sizes without uploading")
parser.add_argument("--local", metavar="DIR", help="Upload rlogs from DIR (<dongle id>|<log id>--<segment>/) instead of the comma API")
args = parser.parse_args()
upload_route(args.route, dry_run=args.dry_run, local_dir=args.local)
upload_route(args.route, dry_run=args.dry_run)
+14 -6
View File
@@ -253,12 +253,6 @@ def hardware_thread(end_event, hw_queue) -> None:
chestnut_status = ChestnutStatus()
branch = get_short_branch()
build_metadata = get_build_metadata()
is_unsupported_combo = COMMA_HARDWARE and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
startup_conditions["not_tici"] = not is_unsupported_combo
onroad_conditions["not_tici"] = not is_unsupported_combo
set_offroad_alert_if_changed("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
@@ -381,6 +375,16 @@ def hardware_thread(end_event, hw_queue) -> None:
startup_conditions["not_always_offroad"] = not offroad_mode
onroad_conditions["not_always_offroad"] = not offroad_mode
# if an unsupported device and branch is detected, going onroad is blocked
# only allow going onroad when:
# - TIZI, or
# - TICI and channel_type is "tici"
build_metadata = get_build_metadata()
is_unsupported_combo = COMMA_HARDWARE and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
startup_conditions["not_tici"] = not is_unsupported_combo
onroad_conditions["not_tici"] = not is_unsupported_combo
set_offroad_alert("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
# if the temperature enters the danger zone, go offroad to cool down
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.critical
extra_text = f"{offroad_comp_temp:.1f}C"
@@ -439,6 +443,10 @@ def hardware_thread(end_event, hw_queue) -> None:
# Offroad power monitoring
voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
# GitHub runner auto off: 9V is used as the threshold because most desktop runners
# will rarely exceed 5V so 9V is set as our buffer between desk use and car use.
params.put_bool("GithubRunnerSufficientVoltage", ((voltage or 0) and voltage > 9000))
power_monitor.calculate(voltage, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
+2 -1
View File
@@ -184,7 +184,8 @@ procs += [
NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
PythonProcess("mapd_manager", "openpilot.sunnypilot.mapd.mapd_manager", always_run),
PythonProcess("gps_fuser", "openpilot.sunnypilot.selfdrive.locationd.gps_fuser", only_onroad),
# locationd
NativeProcess("locationd_llk", "openpilot/sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
]
if os.path.exists("../../sunnypilot/sunnylink/uploader.py"):
+12 -27
View File
@@ -13,41 +13,26 @@ from pathlib import Path
from datetime import datetime, UTC
_VERSION_REGEX = re.compile(r'^(?:\d+[a-zA-Z]+|\d+[vVbB]\d+|[vVbB]\d+|\d{4})$')
_DECIMAL_VERSION_REGEX = re.compile(r'^([vVbB]?)(\d+(?:\.\d+)+)$')
def _version(word: str) -> str | None:
match = _DECIMAL_VERSION_REGEX.match(word.strip('()[]{}.,;:'))
if match:
return match.group(1).upper() + match.group(2)
stripped = re.sub(r'[^a-zA-Z0-9]', '', word)
return stripped.upper() if _VERSION_REGEX.match(stripped) else None
def _truncate(short_name: str) -> str:
return short_name[:8].rstrip('.')
def create_short_name(full_name: str) -> str:
# Remove parentheses and extract alphanumeric words, keeping decimals for versioning
# Remove parentheses and extract alphanumeric words
clean_name = re.sub(r'\([^)]*\)', '', full_name)
words = [re.sub(r'[^a-zA-Z0-9.]', '', word) for word in clean_name.split()]
words = [word for word in words if word]
words = [re.sub(r'[^a-zA-Z0-9]', '', word) for word in clean_name.split() if re.sub(r'[^a-zA-Z0-9]', '', word)]
if len(words) == 1:
return _truncate((_version(words[0]) or words[0]).upper())
return words[0][:8].upper()
# Handle special case: Name + Version (e.g., "Word A1" -> "WordA1", "Model v1.1" -> "ModelV1.1")
if len(words) == 2 and re.match(r'^[A-Za-z]?\d+$', words[1]):
return _truncate((words[0] + (_version(words[1]) or words[1])).upper())
# Handle special case: Name + Version (e.g., "Word A1" -> "WordA1")
if len(words) == 2 and re.match(r'^[A-Za-z]\d+$', words[1]):
return (words[0] + words[1])[:8].upper()
result = ""
for word in words:
# Version or number patterns
version = _version(word)
if version:
result += version
if (re.match(r'^\d+[a-zA-Z]+$', word) or
re.match(r'^\d+[vVbB]\d+$', word) or
re.match(r'^[vVbB]\d+$', word) or
re.match(r'^\d{4}$', word)):
result += word.upper()
# All uppercase abbreviations (2-3 letters)
elif re.match(r'^[A-Z]{2,3}$', word):
result += word
@@ -60,7 +45,7 @@ def create_short_name(full_name: str) -> str:
result += word
else:
result += word[0].upper()
return _truncate(result)
return result[:8]
def create_pkl_name(full_name: str) -> str: