mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-08 09:03:42 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 firestarsdog
|
0bdae4b250 |
@@ -81,7 +81,6 @@ selfdrive/modeld/models/*.pkl
|
||||
# openpilot log files
|
||||
*.bz2
|
||||
*.zst
|
||||
!selfdrive/modeld/firmware/amdgpu/*.zst
|
||||
|
||||
build/
|
||||
|
||||
|
||||
-162
@@ -1,162 +0,0 @@
|
||||
# Credits and Code Provenance
|
||||
|
||||
StarPilot stands on work by comma.ai, FrogPilot, and many other open-source contributors. This
|
||||
document records provenance that is not adequately represented by StarPilot's historical commit
|
||||
authorship. It is not an assertion that an upstream contributor endorses, maintains, or is
|
||||
responsible for StarPilot's adaptation.
|
||||
|
||||
## Ford support adapted from BluePilot
|
||||
|
||||
StarPilot commit [`3f6ccd104e826643887930b41ad3b4086b833d32`](https://github.com/firestar5683/StarPilot/commit/3f6ccd104e826643887930b41ad3b4086b833d32)
|
||||
introduced a substantial adaptation of Ford work from BluePilot's `bp-7.0` branch. That commit's
|
||||
message thanked the project, but it did not record a source revision or preserve the upstream
|
||||
authors in the code. This file corrects that provenance gap without rewriting published history.
|
||||
|
||||
The exact checkout used for the original port was not recorded and therefore cannot now be proven.
|
||||
At the time of the import,
|
||||
[`3a838bba6d3d280592c2fd0496b3378977ae25f1`](https://github.com/BluePilotDev/bluepilot/commit/3a838bba6d3d280592c2fd0496b3378977ae25f1),
|
||||
was the tip of the public `bp-7.0` line. Some imported platform lines instead trace to the
|
||||
contemporaneous development line at
|
||||
[`59e3f2f16a38ff0c16d173b0ccddded23aaa1cd8`](https://github.com/BluePilotDev/bluepilot/commit/59e3f2f16a38ff0c16d173b0ccddded23aaa1cd8).
|
||||
Those histories were merged the next day as
|
||||
[`e1d051d7ba270261b4455068bd68f1a58db15a4a`](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a),
|
||||
which is the complete `bp-7.0` snapshot used for this audit. This reconstruction is deliberately
|
||||
recorded as a range rather than pretending that the missing original source SHA can be recovered.
|
||||
|
||||
### Upstream authors and work
|
||||
|
||||
- **Alan Polk (`alan-polk`)** is the principal upstream author of the Ford curvature controller,
|
||||
angle-primary controller, manual-turn behavior, controller integration, and related panda safety
|
||||
work. Important lineage commits include
|
||||
[`db2bdff05`](https://github.com/BluePilotDev/bluepilot/commit/db2bdff05df103d71df62f45c2a3cb5211aba6e6),
|
||||
[`d0aac605f`](https://github.com/BluePilotDev/bluepilot/commit/d0aac605f99d37e9da205e419f7989c1e9eaa386),
|
||||
[`8f8d6d15f`](https://github.com/BluePilotDev/bluepilot/commit/8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d), and
|
||||
[`97867c1eb`](https://github.com/BluePilotDev/bluepilot/commit/97867c1eb57b7472f6fc3de62f0fef576e5a5497).
|
||||
- **John Christman** contributed `bp-7.0` lateral integration, platform data, and the upstream
|
||||
anti-stall work referenced by StarPilot's recovery logic, including
|
||||
[`ec0ab181c`](https://github.com/BluePilotDev/bluepilot/commit/ec0ab181c344dccaae053e763bc6ce269551a2d8) and
|
||||
[`9012f7666`](https://github.com/BluePilotDev/bluepilot/commit/9012f76666a5c90764fcaec40832b9a607488c27),
|
||||
with additional vehicle data in
|
||||
[`f0c6bf51f`](https://github.com/BluePilotDev/bluepilot/commit/f0c6bf51f19bb7b74226ead622cabef56e602125).
|
||||
- **Jacob Neulight** contributed measured shadow-curvature and steering-pinion curvature/safety
|
||||
work, including
|
||||
[`699c17d9f`](https://github.com/BluePilotDev/bluepilot/commit/699c17d9fde62c690eed9d68eed7d731744d5137) and
|
||||
[`26030f3cb`](https://github.com/BluePilotDev/bluepilot/commit/26030f3cb56a169ab0cf2b5a5ad15fa9af391b10).
|
||||
- **Nathan Ingraham** contributed the separate high-speed damping adjustment in
|
||||
[`1db52bc79`](https://github.com/BluePilotDev/bluepilot/commit/1db52bc79e607ddb00214254dd4d732615e27fe4) and
|
||||
[`3610e3f18`](https://github.com/BluePilotDev/bluepilot/commit/3610e3f18a0ad37d793d7ada61dbc739ab1077a3).
|
||||
- **Praeuner** contributed the damping-range update in
|
||||
[`b600a8fdb`](https://github.com/BluePilotDev/bluepilot/commit/b600a8fdb985a2219fad4006ce9444b7071b52da).
|
||||
- **tonesto7** contributed to the Ford integration and CAN-message history represented in the
|
||||
imported branch, including
|
||||
[`7f9212f7f`](https://github.com/BluePilotDev/bluepilot/commit/7f9212f7f8dc22c4a0a22443158554f4d3652a3c).
|
||||
- **Haibin Wen and sunnypilot contributors** are named in file-level notices on upstream Ford
|
||||
extension files that informed StarPilot's vehicle-state and lateral-limit integration. Their
|
||||
published copyright and license notices are retained in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md).
|
||||
- **Dirk Petersen, Alex Troxel, Kacer Aleks, and other BluePilot contributors** supplied Ford
|
||||
fingerprints, VIN/platform data, tests, and integration changes that are represented in the
|
||||
imported vehicle-support set. Examples include
|
||||
[`3706b5645`](https://github.com/BluePilotDev/bluepilot/commit/3706b5645c270ad83cfecc3e92618924f0963166),
|
||||
[`694bdbb7a`](https://github.com/BluePilotDev/bluepilot/commit/694bdbb7a9cc70af04d0118e891df96e47ded7c7), and
|
||||
[`a02c06ef3`](https://github.com/BluePilotDev/bluepilot/commit/a02c06ef37acff518a511ba0a88a4dc65f506353).
|
||||
|
||||
This list identifies contributors whose work was found during the repository and blame audit. The
|
||||
BluePilot repository and its Git history remain the authoritative record and may identify further
|
||||
contributors.
|
||||
|
||||
### Local-to-upstream map
|
||||
|
||||
| StarPilot area | Upstream lineage | What StarPilot changed |
|
||||
| --- | --- | --- |
|
||||
| `starpilot/car/ford/lateral.py` | `human_turn.py`, `lateral_curv_ext.py`, and `values_ext.py` at the reference snapshot above; earlier StarPilot revisions also adapted `lateral_angle_ext.py` | Consolidated the runtime implementation on the extended-curvature strategy, integrated StarPilot Params, and added live-delay curvature lookahead and local tuning behavior. |
|
||||
| `starpilot/car/ford/fordcan.py` | `fordcan_ext.py` and the extended-lateral protocol, especially `8f8d6d15f` | Reduced the extension to the curvature CAN constructors used by StarPilot and adapted it to the local controller interface. |
|
||||
| `opendbc_repo/opendbc/car/ford/` | Ford controller/state/interface/radar/platform changes in the `bp-7.0` snapshot | Integrated the changes directly into StarPilot's opendbc layout instead of retaining sunnypilot mixins; subsequent fixes and behavior differ by file. |
|
||||
| `opendbc_repo/opendbc/safety/modes/ford.h` and Ford safety tests | BluePilot panda enforcement for four-signal curvature and angle-primary control, especially `8f8d6d15f`, plus shadow-curvature work | Retained the extended-curvature checks, removed runtime path-angle selection, and continued adding local regression coverage. |
|
||||
| Ford Params and settings surfaces | BluePilot's curvature tuning concepts | Renamed and implemented in StarPilot's native Params/Galaxy architecture; no BluePilot or sunnypilot UI classes were retained. |
|
||||
|
||||
The local code has materially diverged, but the first four rows remain derivative in design and in
|
||||
parts of their implementation. Future ports should cite the exact upstream commit in the importing
|
||||
commit and at the relevant source boundary; when history can be retained cleanly, use a merge,
|
||||
subtree, or cherry-pick with origin metadata rather than a single squashed attribution.
|
||||
|
||||
## Hyundai, Kia, and Genesis support adapted from sunnypilot
|
||||
|
||||
Portions of StarPilot's HKG angle steering, CAN integration, panda safety enforcement, safety tests,
|
||||
firmware fingerprints, and cruise-button management are adapted from sunnypilot. Some upstream HKG
|
||||
authorship is retained in StarPilot's Git history, and StarPilot commit
|
||||
[`e33305151`](https://github.com/firestar5683/StarPilot/commit/e33305151ba852ea3350aa9f7a12d1c8bd137c43)
|
||||
identified its ICBM/CSLC work as a sunnypilot port. Other substantial imports were committed locally
|
||||
without recording an exact source revision, so this section documents the reconstructed lineage.
|
||||
|
||||
The exact checkout used for each historical import cannot now be proven. The reference snapshots
|
||||
used for this audit are:
|
||||
|
||||
- [`sunnypilot/sunnypilot` `hkg-angle-steering-2025`](https://github.com/sunnypilot/sunnypilot/tree/hkg-angle-steering-2025)
|
||||
at [`cfb38312d`](https://github.com/sunnypilot/sunnypilot/commit/cfb38312db33779f4727c983d372474a56ccb5d8),
|
||||
whose opendbc submodule points to the next snapshot.
|
||||
- [`sunnypilot/opendbc` `hkg-angle-steering-2025`](https://github.com/sunnypilot/opendbc/tree/hkg-angle-steering-2025)
|
||||
at [`cc4b08625`](https://github.com/sunnypilot/opendbc/commit/cc4b08625a98e94b318cab15e45e05dad58042bd).
|
||||
- [`sunnypilot/opendbc` `master`](https://github.com/sunnypilot/opendbc)
|
||||
at [`f95f996f5`](https://github.com/sunnypilot/opendbc/commit/f95f996f5917dcbbf2e32fe51b606a24cf836af6),
|
||||
used to audit later HKG fingerprints and extension history.
|
||||
|
||||
### Upstream authors and work
|
||||
|
||||
- **Haibin (Jason) Wen** contributed the original Kia EV9 HDA2/LFA2 angle-steering port, Hyundai
|
||||
angle integration and signals, non-SCC platform support, and Intelligent Cruise Button Management.
|
||||
Important lineage commits include
|
||||
[`abe78de2c`](https://github.com/sunnypilot/opendbc/commit/abe78de2c2f9d8774d343c35c181d27e9d944392),
|
||||
[`333de6f1a`](https://github.com/sunnypilot/opendbc/commit/333de6f1a2097a27709a652d5862bad05d83ed1f),
|
||||
[`559a37426`](https://github.com/sunnypilot/opendbc/commit/559a37426976171ceb56c541bec9362abd8b8bd2), and
|
||||
[`862828ad6`](https://github.com/sunnypilot/opendbc/commit/862828ad6f870fb23dd8670fe2a18dc19313217b).
|
||||
- **Shane Smiskol** contributed foundational angle-command limiting, driver-override behavior, and
|
||||
EPS-fault avoidance, including
|
||||
[`228a397a3`](https://github.com/sunnypilot/opendbc/commit/228a397a37618de1ecbaf06b70efe3e0e0a8eec6) and
|
||||
[`42d84ff6c`](https://github.com/sunnypilot/opendbc/commit/42d84ff6ca3d48529b195395d652ad777ad397ca).
|
||||
- **DevTekVE** contributed substantial angle-controller integration, tuning, platform support, panda
|
||||
safety logic, and tests, including
|
||||
[`c77c7ec2e`](https://github.com/sunnypilot/opendbc/commit/c77c7ec2e39959b65c42e2d70aa943fccd606361),
|
||||
[`7ded99dba`](https://github.com/sunnypilot/opendbc/commit/7ded99dba145c56754e8bbe47217eb174ec03396),
|
||||
[`3819ca7f0`](https://github.com/sunnypilot/opendbc/commit/3819ca7f0d2576771924bd60f47d3e8676b5d583), and
|
||||
[`8d134e98f`](https://github.com/sunnypilot/opendbc/commit/8d134e98f3151a1aec354f93e81c3a4269788991).
|
||||
- **Nicholas Evans, dany7915, janpoo6427, Tinkerpet, royjr, Taylor Hoshino, Intelli, Joshua Mack,
|
||||
Mark McCallister, Discountchubbs, and other sunnypilot contributors** supplied vehicle ports,
|
||||
fingerprints, firmware data, safety-limit updates, and related integration represented in the
|
||||
adapted HKG support.
|
||||
|
||||
This list identifies contributors found during the repository, commit, and blame audit. The
|
||||
sunnypilot repositories and their Git histories remain the authoritative record and may identify
|
||||
additional contributors.
|
||||
|
||||
### Local-to-upstream map
|
||||
|
||||
| StarPilot area | Upstream lineage | What StarPilot changed |
|
||||
| --- | --- | --- |
|
||||
| `opendbc_repo/opendbc/car/hyundai/carcontroller.py` | Angle controller and driver-override work in `sunnypilot/opendbc` `hkg-angle-steering-2025` | Integrated the controller into StarPilot's opendbc layout and added substantial local longitudinal, smoothing, recovery, and platform behavior. |
|
||||
| `opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py`, `interface.py`, and `values.py` | Angle commands, signal selection, safety flags, limits, and platform integration from the angle branch | Combined later upstream changes with StarPilot flags, Params, platform tuning, and local CAN/CAN-FD behavior. |
|
||||
| `opendbc_repo/opendbc/safety/modes/hyundai_canfd.h` and its tests | Angle-command safety enforcement and regression tests from the angle branch | Extended and reorganized the safety mode and tests for StarPilot's current supported modes. |
|
||||
| `opendbc_repo/opendbc/car/hyundai/fingerprints.py` and HKG platform data | sunnypilot HKG extension/fingerprint history on `master` plus angle-branch vehicle ports | Flattened extension data into the local opendbc tree and continued adding and updating platforms. |
|
||||
| HKG cruise-button management and settings integration | sunnypilot ICBM/CSLC concepts and implementation history, including `862828ad6` and `2277e3d49` | Adapted the feature to StarPilot/FrogPilot controls and settings; later revisions changed or removed portions of the original integration. |
|
||||
|
||||
The current implementation is not a wholesale copy of either reference snapshot and has diverged
|
||||
substantially. Its HKG angle-control and supporting safety architecture nevertheless remain
|
||||
derivative in design and in identifiable portions of the implementation. The applicable upstream
|
||||
notices are preserved in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md).
|
||||
|
||||
## Policy for future third-party ports
|
||||
|
||||
Before publishing a third-party port:
|
||||
|
||||
1. Read every repository-level and file-level license that may cover the source. Stop and ask the
|
||||
rightsholder when the terms or their scope conflict.
|
||||
2. Record the repository URL, exact commit SHA, source paths/symbols, authors found in the relevant
|
||||
history, and the local destination in the importing commit and this file.
|
||||
3. Preserve required copyright and license notices verbatim. Reorganization, translation, and
|
||||
AI-assisted rewriting do not remove source provenance.
|
||||
4. Retain authorship/history with a merge, subtree, or `cherry-pick -x` when practical. For a true
|
||||
adaptation, keep the local adapter as commit author and use an `Adapted-from:` trailer and source
|
||||
comments; do not add `Co-authored-by:` for a person without their agreement.
|
||||
5. Describe material local deviations and make clear that upstream contributors do not support or
|
||||
endorse the downstream version.
|
||||
|
||||
See [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md) for licensing notices.
|
||||
@@ -1,5 +1,4 @@
|
||||
Copyright (c) 2018, Comma.ai, Inc.
|
||||
Copyright (c) 2026, firestar5683 and StarPilot contributors
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
||||
|
||||
|
||||
@@ -21,20 +21,6 @@ but [has expanded to offer Quality-Of-Life improvements for all](#features)!
|
||||
StarPilot is built off of [FrogPilot](https://github.com/FrogAi/FrogPilot)
|
||||
and supports the major features FrogPilot offers.
|
||||
|
||||
Ford-specific lateral-control and vehicle-support work includes substantial adaptations from
|
||||
[BluePilot](https://github.com/BluePilotDev/bluepilot/tree/bp-7.0), principally developed by
|
||||
[Alan Polk](https://github.com/alan-polk) with additional BluePilot contributors. StarPilot's
|
||||
implementation has since diverged, but that does not erase its lineage. See [CREDITS.md](CREDITS.md)
|
||||
for the code-level provenance and [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md) for applicable
|
||||
upstream notices and terms. BluePilot and its contributors do not maintain or endorse StarPilot;
|
||||
please direct support requests for this adaptation to the StarPilot project.
|
||||
|
||||
Hyundai, Kia, and Genesis angle steering and related vehicle support include substantial adaptations
|
||||
from [sunnypilot](https://github.com/sunnypilot/sunnypilot/tree/hkg-angle-steering-2025) and its
|
||||
[opendbc angle-steering branch](https://github.com/sunnypilot/opendbc/tree/hkg-angle-steering-2025).
|
||||
StarPilot's implementation has diverged significantly; the upstream contributors do not maintain
|
||||
this adaptation. Detailed code lineage is recorded in [CREDITS.md](CREDITS.md#hyundai-kia-and-genesis-support-adapted-from-sunnypilot).
|
||||
|
||||
StarPilot has a vibrant, welcoming community [discord](https://firestar.link/discord).
|
||||
Stop by to chat or ask questions!
|
||||
|
||||
@@ -91,9 +77,4 @@ Uses your comma's sysroot/toolchain
|
||||
* Custom long maneuver tests, specifically designed for regen-only vehicles
|
||||
|
||||
## Third-Party Notices
|
||||
|
||||
* Portions of this software include modified versions of the Material Design Icons provided by Google under the Apache License 2.0. A copy of the license is included in the `LICENSE-MDI` file.
|
||||
* Ford support includes software adapted from BluePilot's `bp-7.0` branch. The source repository contains both a standard MIT notice and a separate custom SUNNYPILOT LLC notice; StarPilot preserves both in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md).
|
||||
* Hyundai, Kia, and Genesis support includes software adapted directly from sunnypilot's HKG angle-steering branch and sunnypilot/opendbc. StarPilot preserves the applicable notices in [THIRD_PARTY_NOTICES.md](THIRD_PARTY_NOTICES.md).
|
||||
|
||||
> This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use.
|
||||
|
||||
@@ -1,125 +0,0 @@
|
||||
# Third-Party Notices
|
||||
|
||||
This file supplements StarPilot's root `LICENSE`. StarPilot-original contributions are offered
|
||||
under that MIT license. Third-party material remains subject to its original terms; inclusion here
|
||||
does not relicense it or imply endorsement by an upstream project or contributor.
|
||||
|
||||
## BluePilot `bp-7.0` Ford support
|
||||
|
||||
Portions of StarPilot's Ford lateral control, CAN integration, panda safety logic, vehicle data,
|
||||
and related tests are adapted from the public BluePilot repository:
|
||||
|
||||
- Source: <https://github.com/BluePilotDev/bluepilot/tree/bp-7.0>
|
||||
- Audited `bp-7.0` snapshot: [`e1d051d7ba270261b4455068bd68f1a58db15a4a`](https://github.com/BluePilotDev/bluepilot/commit/e1d051d7ba270261b4455068bd68f1a58db15a4a)
|
||||
- Historical reconstruction: [CREDITS.md](CREDITS.md#ford-support-adapted-from-bluepilot)
|
||||
- Provenance and contributors: [CREDITS.md](CREDITS.md)
|
||||
|
||||
## sunnypilot Hyundai, Kia, and Genesis support
|
||||
|
||||
Portions of StarPilot's HKG angle steering, CAN integration, panda safety enforcement and tests,
|
||||
firmware fingerprints, platform data, and cruise-button management are adapted directly from the
|
||||
public sunnypilot repositories:
|
||||
|
||||
- Source: <https://github.com/sunnypilot/sunnypilot/tree/hkg-angle-steering-2025>
|
||||
- Audited superproject snapshot: [`cfb38312db33779f4727c983d372474a56ccb5d8`](https://github.com/sunnypilot/sunnypilot/commit/cfb38312db33779f4727c983d372474a56ccb5d8)
|
||||
- Source: <https://github.com/sunnypilot/opendbc/tree/hkg-angle-steering-2025>
|
||||
- Audited angle-branch snapshot: [`cc4b08625a98e94b318cab15e45e05dad58042bd`](https://github.com/sunnypilot/opendbc/commit/cc4b08625a98e94b318cab15e45e05dad58042bd)
|
||||
- Later HKG history was audited against `sunnypilot/opendbc` `master` at
|
||||
[`f95f996f5917dcbbf2e32fe51b606a24cf836af6`](https://github.com/sunnypilot/opendbc/commit/f95f996f5917dcbbf2e32fe51b606a24cf836af6).
|
||||
- Historical reconstruction and contributors: [CREDITS.md](CREDITS.md#hyundai-kia-and-genesis-support-adapted-from-sunnypilot)
|
||||
|
||||
## Applicable upstream license notices
|
||||
|
||||
The upstream repositories and branches identified above publish both `LICENSE` and `LICENSE.md`.
|
||||
Their READMEs point to `LICENSE` for openpilot licensing, while some extension files point to
|
||||
`LICENSE.md`. Because the scope of those two notices is not unambiguous, StarPilot preserves both
|
||||
and treats the more restrictive notice conservatively for upstream-derived material. This is a
|
||||
record of the published notices, not a legal conclusion about their scope.
|
||||
|
||||
### Upstream `LICENSE` notice (MIT)
|
||||
|
||||
Copyright (c) 2018, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
|
||||
associated documentation files (the "Software"), to deal in the Software without restriction,
|
||||
including without limitation the rights to use, copy, modify, merge, publish, distribute,
|
||||
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or
|
||||
substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT
|
||||
NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||||
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
|
||||
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT
|
||||
OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
### Upstream `opendbc` `LICENSE` notice (MIT)
|
||||
|
||||
Copyright (c) 2020, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
|
||||
associated documentation files (the "Software"), to deal in the Software without restriction,
|
||||
including without limitation the rights to use, copy, modify, merge, publish, distribute,
|
||||
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or
|
||||
substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT
|
||||
NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||||
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
|
||||
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT
|
||||
OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
### Upstream extension file notice
|
||||
|
||||
Several upstream Ford and HKG extension files carry this file-level notice:
|
||||
|
||||
> 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.
|
||||
|
||||
StarPilot's Ford and HKG integrations were informed by extension files carrying this notice. The
|
||||
reference to “MIT License” conflicts with the nonstandard restrictions in the referenced upstream
|
||||
`LICENSE.md`; both texts are retained here rather than silently choosing between them.
|
||||
|
||||
### Upstream `LICENSE.md` notice (published as “Custom MIT License”)
|
||||
|
||||
The following notice is reproduced verbatim from the reference branch:
|
||||
|
||||
> # Custom MIT License
|
||||
>
|
||||
> Copyright (c) 2024, Haibin Wen, SUNNYPILOT LLC
|
||||
>
|
||||
> Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to view and modify the Software, subject to the following conditions:
|
||||
>
|
||||
> 1. **Permission Required**: Permission Required for Commercial, For-Profit, or Closed Source Use: Use of the Software, in whole or in part, for any commercial purposes, for-profit projects, or in closed source projects requires explicit written permission from the original author(s).
|
||||
>
|
||||
> 2. **Redistribution**: Any redistribution of the Software, modified or unmodified, must retain this license notice and the following acknowledgment:
|
||||
> "This software is licensed under a custom license requiring permission for use."
|
||||
>
|
||||
> 3. **Visibility**: Any project that uses the Software must visibly mention the following acknowledgment:
|
||||
> "This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use."
|
||||
>
|
||||
> 4. **No Warranty**: THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
>
|
||||
> Contact sunnypilot Support <support@sunnypilot.ai> for permission requests.
|
||||
>
|
||||
> ---
|
||||
>
|
||||
> Haibin Wen, SUNNYPILOT LLC
|
||||
|
||||
Required upstream acknowledgments:
|
||||
|
||||
> This software is licensed under a custom license requiring permission for use.
|
||||
>
|
||||
> This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use.
|
||||
|
||||
For commercial, for-profit, or closed-source use, consult the upstream notice and obtain any
|
||||
permission it requires. The upstream repository's simultaneous publication of two differently
|
||||
scoped notices should be clarified with the relevant copyright holders before relying on one to
|
||||
the exclusion of the other.
|
||||
Binary file not shown.
@@ -774,7 +774,6 @@ struct ChestnutState {
|
||||
pcieLtssm @7 :UInt8;
|
||||
supplyVoltage @8 :UInt16; # mV
|
||||
supplyCurrent @9 :Int16; # mA
|
||||
supplyFault @10 :Bool;
|
||||
}
|
||||
|
||||
struct RadarState @0x9a185389d6fdd05f {
|
||||
|
||||
+1
-15
@@ -1,22 +1,8 @@
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
def _gm_car_params_present(params: Params) -> bool:
|
||||
try:
|
||||
raw_car_params = params.get("CarParams")
|
||||
if raw_car_params is None:
|
||||
return False
|
||||
with car.CarParams.from_bytes(raw_car_params) as CP:
|
||||
return CP.brand == "gm"
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
def get_gps_location_service(params: Params) -> str:
|
||||
# GM card publishes the selected device/PPS/OnStar result through the
|
||||
# existing external service. Keep every other platform on its prior rule.
|
||||
if _gm_car_params_present(params) or params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
|
||||
if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
|
||||
return "gpsLocationExternal"
|
||||
else:
|
||||
return "gpsLocation"
|
||||
|
||||
Binary file not shown.
+7
-21
@@ -113,12 +113,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||
{"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
|
||||
{"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
|
||||
{"Offroad_ChestnutNotDetected", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
|
||||
{"Offroad_ChestnutOverheated", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"Offroad_ChestnutPcieUnavailable", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"Offroad_ChestnutUncompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
|
||||
{"Offroad_ChestnutUpdateFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
|
||||
{"Offroad_ChestnutUsbSlow", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
|
||||
{"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"Offroad_ExcessiveActuation", {PERSISTENT, JSON}},
|
||||
@@ -316,7 +310,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DeveloperSidebarMetric7", {PERSISTENT, INT, "7", "0", 3}},
|
||||
{"DeveloperUI", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
{"GalaxyDeveloperMode", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
|
||||
{"GalaxyMobileDefault", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_ADVANCED}},
|
||||
{"DeveloperWidgets", {PERSISTENT, BOOL, "1", "0", 3}},
|
||||
{"DeviceManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"DeviceShutdown", {PERSISTENT, INT, "6", "6", 1, SETTINGS_SIMPLE}},
|
||||
@@ -337,12 +330,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DownloadAllModels", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
|
||||
{"DownloadMaps", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
|
||||
{"DriverCamera", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
|
||||
{"ActiveBigModel", {PERSISTENT, STRING}},
|
||||
{"ActiveBigModelName", {PERSISTENT, STRING}},
|
||||
{"ActiveBigModelVersion", {PERSISTENT, STRING}},
|
||||
{"ActiveSmallModel", {PERSISTENT, STRING}},
|
||||
{"ActiveSmallModelName", {PERSISTENT, STRING}},
|
||||
{"ActiveSmallModelVersion", {PERSISTENT, STRING}},
|
||||
{"Model", {PERSISTENT, STRING, "rdf43", "rdf43", 1}},
|
||||
{"ModelVersion", {PERSISTENT, STRING, "v15", "v15", 1}},
|
||||
{"DrivingModel", {PERSISTENT, STRING, "rdf43", "rdf43", 1}},
|
||||
@@ -385,13 +372,17 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"FordLKASButtonControlMigrated", {PERSISTENT, BOOL, "0", "0"}},
|
||||
{"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}},
|
||||
// These Ford curvature tuning concepts descend from BluePilot bp-7.0. StarPilot's key names and
|
||||
// settings integration are local; see /CREDITS.md and /THIRD_PARTY_NOTICES.md for provenance.
|
||||
{"FordAngleBlend", {PERSISTENT, FLOAT, "0.5", "0.5", 2}},
|
||||
{"FordAngleHighSpeedDamping", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
|
||||
{"FordAngleHighSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
|
||||
{"FordAngleLaneChangeFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
|
||||
{"FordAngleLowSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
|
||||
{"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
|
||||
{"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
|
||||
{"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}},
|
||||
{"FordHandsFreeCluster", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}},
|
||||
{"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}},
|
||||
{"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}},
|
||||
{"FLMActiveProfileId", {PERSISTENT, STRING, "", "", 2}},
|
||||
{"FLMSubmittedTune", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
|
||||
@@ -519,9 +510,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ModeButtonControl", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"ModelDownloadProgress", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"ModelDrivesAndScores", {PERSISTENT, JSON, "{}", "{}"}},
|
||||
{"ModelLabConfig", {PERSISTENT, JSON, "{}", "{}"}},
|
||||
{"ModelLabModelToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
{"ModelLabRuntime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}},
|
||||
{"ModelReleasedDates", {PERSISTENT, STRING, "", "", 1}},
|
||||
{"ModelRandomizer", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"LatSmoothSeconds", {PERSISTENT, FLOAT, "0.1", "0.1", 3}},
|
||||
@@ -556,8 +544,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"FavoriteTrafficModeCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"WheelButtonBookmarkCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"WheelControlAOLCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"WheelControlDisengageCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"WheelControlEngageCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"WheelControlForceCoastCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"WheelControlPulseGlideCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}},
|
||||
{"openpilotMinutes", {PERSISTENT, INT, "0", "0", 0}},
|
||||
@@ -719,7 +705,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SubaruSNG", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruSNGManualParkingBrake", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruStopStartOff", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"SubaruAvhOnAtStartup", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
|
||||
{"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
|
||||
|
||||
Binary file not shown.
+1
-1
@@ -21,7 +21,7 @@ fi
|
||||
export QCOM_PRIORITY=12
|
||||
|
||||
if [ -z "$AGNOS_VERSION" ]; then
|
||||
export AGNOS_VERSION="19.6.20"
|
||||
export AGNOS_VERSION="19.6.19"
|
||||
fi
|
||||
|
||||
if [ -z "$AGNOS_ACCEPTED_VERSIONS" ]; then
|
||||
|
||||
@@ -10,7 +10,6 @@ from opendbc.car.carlog import carlog
|
||||
from opendbc.car.structs import CarParams, CarParamsT
|
||||
from opendbc.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
|
||||
from opendbc.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car
|
||||
from opendbc.car.hyundai.values import kia_ray_ev_vin
|
||||
from opendbc.car.mock.values import CAR as MOCK
|
||||
from opendbc.car.toyota.values import ToyotaSafetyFlags
|
||||
from opendbc.car.values import BRANDS
|
||||
@@ -247,13 +246,8 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu
|
||||
set_obd_multiplexing(True)
|
||||
# VIN query only reliably works through OBDII
|
||||
vin_rx_addr, vin_rx_bus, vin = get_vin(can_recv, can_send, (0, 1))
|
||||
skip_fw_buses = {1} if kia_ray_ev_vin(vin) else set()
|
||||
if skip_fw_buses:
|
||||
carlog.warning("Kia Ray EV: skipping CAN1 firmware queries")
|
||||
ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing,
|
||||
num_pandas=num_pandas, skip_buses=skip_fw_buses)
|
||||
car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs,
|
||||
num_pandas=num_pandas, skip_buses=skip_fw_buses)
|
||||
ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing, num_pandas=num_pandas)
|
||||
car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas)
|
||||
cached = False
|
||||
|
||||
exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin)
|
||||
|
||||
@@ -1,15 +1,13 @@
|
||||
import math
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, structs
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs
|
||||
from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
|
||||
from opendbc.car.ford import fordcan
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR
|
||||
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
|
||||
# This Ford extension boundary substantially adapts BluePilot bp-7.0 work. See the root CREDITS.md
|
||||
# (including Alan Polk's d0aac605f and db2bdff05) and THIRD_PARTY_NOTICES.md.
|
||||
from openpilot.starpilot.car.ford import fordcan as starpilot_fordcan
|
||||
from openpilot.starpilot.car.ford.lateral import FordLateralController, FordLateralResult
|
||||
from openpilot.starpilot.car.ford.lateral import FordLateralController, FordLateralMode, FordLateralResult
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
@@ -45,6 +43,22 @@ def apply_ford_angle(desired_angle_deg: float, current_angle_deg: float) -> floa
|
||||
return float(np.clip(relative_angle, -5.8, 5.8))
|
||||
|
||||
|
||||
def anti_overshoot(apply_curvature, apply_curvature_last, v_ego):
|
||||
diff = 0.1
|
||||
tau = 5 # 5s smooths over the overshoot
|
||||
dt = DT_CTRL * CarControllerParams.STEER_STEP
|
||||
alpha = 1 - np.exp(-dt / tau)
|
||||
|
||||
lataccel = apply_curvature * (v_ego ** 2)
|
||||
last_lataccel = apply_curvature_last * (v_ego ** 2)
|
||||
last_lataccel = apply_hysteresis(lataccel, last_lataccel, diff)
|
||||
last_lataccel = alpha * lataccel + (1 - alpha) * last_lataccel
|
||||
|
||||
output_curvature = last_lataccel / (max(v_ego, 1) ** 2)
|
||||
|
||||
return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature]))
|
||||
|
||||
|
||||
def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP):
|
||||
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
|
||||
if v_ego_raw > 9:
|
||||
@@ -79,6 +93,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
self.apply_curvature_last = 0
|
||||
self.apply_angle_last = 0
|
||||
self.anti_overshoot_curvature_last = 0
|
||||
self.accel = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake_request = False
|
||||
@@ -88,7 +103,8 @@ class CarController(CarControllerBase):
|
||||
self.lead_distance_bars_last = None
|
||||
self.distance_bar_frame = 0
|
||||
self.ford_lateral = None if CP.flags & FordFlags.LKA_STEERING else FordLateralController(CP)
|
||||
self.ford_extended_lateral_announced = False
|
||||
self.ford_shadow_curvature = 0.0
|
||||
self.ford_lateral_announced_mode = FordLateralMode.native
|
||||
self.stock_cruise_button = FordStockCruiseButton()
|
||||
|
||||
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
||||
@@ -155,26 +171,70 @@ class CarController(CarControllerBase):
|
||||
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, active=lka_active, apply_angle=self.apply_angle_last,
|
||||
direction=direction, ramp_type=ramp_type, curvature=-self.apply_curvature_last))
|
||||
else:
|
||||
if (self.frame % CarControllerParams.STEER_STEP) == 0:
|
||||
lateral = self.ford_lateral.update(CC, CS, actuators) \
|
||||
if self.ford_extended_lateral_announced else FordLateralResult()
|
||||
lateral_mode = self.ford_lateral.mode
|
||||
lateral_mode_ready = lateral_mode == self.ford_lateral_announced_mode
|
||||
|
||||
# Keep the original Ford path available without changing its command behavior.
|
||||
if lateral_mode == FordLateralMode.native:
|
||||
if (self.frame % CarControllerParams.STEER_STEP) == 0:
|
||||
if not lateral_mode_ready:
|
||||
self.apply_curvature_last = 0.0
|
||||
apply_curvature = 0.0
|
||||
elif self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14):
|
||||
self.anti_overshoot_curvature_last = anti_overshoot(
|
||||
actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw)
|
||||
apply_curvature = self.anti_overshoot_curvature_last
|
||||
else:
|
||||
apply_curvature = actuators.curvature
|
||||
|
||||
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
|
||||
self.apply_curvature_last = apply_ford_curvature_limits(
|
||||
apply_curvature, self.apply_curvature_last, current_curvature,
|
||||
CS.out.vEgoRaw, 0., CC.latActive and lateral_mode_ready, self.CP)
|
||||
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
mode = 1 if CC.latActive and lateral_mode_ready else 0
|
||||
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
|
||||
can_sends.append(fordcan.create_lat_ctl2_msg(
|
||||
self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter))
|
||||
else:
|
||||
can_sends.append(fordcan.create_lat_ctl_msg(
|
||||
self.packer, self.CAN, CC.latActive and lateral_mode_ready, 0., 0., -self.apply_curvature_last, 0.))
|
||||
elif (self.frame % CarControllerParams.STEER_STEP) == 0:
|
||||
if not lateral_mode_ready:
|
||||
lateral = FordLateralResult(shadow_curvature=self.ford_lateral._current_curvature(CS))
|
||||
elif lateral_mode == FordLateralMode.angle:
|
||||
lateral = self.ford_lateral.update_angle(CC, CS, actuators)
|
||||
else:
|
||||
lateral = self.ford_lateral.update_curvature(CC, CS, actuators)
|
||||
|
||||
self.apply_curvature_last = lateral.curvature
|
||||
self.ford_shadow_curvature = lateral.shadow_curvature
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
|
||||
can_sends.append(starpilot_fordcan.create_lat_ctl2_msg(
|
||||
self.packer, self.CAN, 1 if lateral.active else 0,
|
||||
lateral.ramp_type, lateral.precision_type,
|
||||
-lateral.path_offset, -lateral.path_angle,
|
||||
-lateral.curvature, -lateral.curvature_rate, counter))
|
||||
else:
|
||||
can_sends.append(starpilot_fordcan.create_lat_ctl_msg(
|
||||
self.packer, self.CAN, lateral.active,
|
||||
lateral.ramp_type, lateral.precision_type,
|
||||
-lateral.path_offset, -lateral.path_angle,
|
||||
-lateral.curvature, -lateral.curvature_rate))
|
||||
|
||||
if (self.frame % CarControllerParams.LKA_STEP) == 0:
|
||||
can_sends.append(starpilot_fordcan.create_lka_msg(self.packer, self.CAN))
|
||||
self.ford_extended_lateral_announced = True
|
||||
if lateral_mode == FordLateralMode.native:
|
||||
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
|
||||
else:
|
||||
angle_mode = lateral_mode == FordLateralMode.angle
|
||||
shadow_curvature = -self.ford_lateral._current_curvature(CS)
|
||||
if angle_mode:
|
||||
shadow_curvature = -self.ford_shadow_curvature
|
||||
can_sends.append(starpilot_fordcan.create_lka_msg(
|
||||
self.packer, self.CAN, angle_mode=angle_mode, shadow_curvature=shadow_curvature))
|
||||
self.ford_lateral_announced_mode = lateral_mode
|
||||
|
||||
### longitudinal control ###
|
||||
# send acc msg at 50Hz
|
||||
@@ -232,7 +292,8 @@ class CarController(CarControllerBase):
|
||||
show_distance_bars = self.frame - self.distance_bar_frame < 400
|
||||
hands_free_cluster = bool(
|
||||
self.ford_lateral is not None
|
||||
and self.ford_extended_lateral_announced
|
||||
and self.ford_lateral.mode != FordLateralMode.native
|
||||
and self.ford_lateral.mode == self.ford_lateral_announced_mode
|
||||
and self.ford_lateral.hands_free_cluster_enabled)
|
||||
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
|
||||
fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
|
||||
|
||||
@@ -1,6 +1,3 @@
|
||||
# Ford-specific additions first imported in StarPilot 3f6ccd104e substantially adapt BluePilot
|
||||
# bp-7.0 vehicle-state work, including a sunnypilot extension with the Haibin Wen/contributors
|
||||
# notice retained in THIRD_PARTY_NOTICES.md. See the repository root CREDITS.md for provenance.
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
# Some Ford firmware entries first imported in StarPilot 3f6ccd104e came from BluePilot bp-7.0
|
||||
# contributors. The exact authors and source revisions are recorded in the root CREDITS.md.
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.ford.values import CAR
|
||||
|
||||
|
||||
@@ -1,5 +1,3 @@
|
||||
# Ford-specific additions first imported in StarPilot 3f6ccd104e substantially adapt BluePilot
|
||||
# bp-7.0 interface work. See the repository root CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
import numpy as np
|
||||
from opendbc.car import Bus, get_safety_config, structs
|
||||
from opendbc.car.carlog import carlog
|
||||
|
||||
@@ -1,5 +1,3 @@
|
||||
# Ford-specific additions first imported in StarPilot 3f6ccd104e substantially adapt BluePilot
|
||||
# bp-7.0 radar work. See the repository root CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
from typing import cast
|
||||
|
||||
@@ -1,5 +1,3 @@
|
||||
# Ford platform data and limits first imported in StarPilot 3f6ccd104e substantially adapt
|
||||
# BluePilot bp-7.0 contributor work. See the repository root CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
import copy
|
||||
import re
|
||||
from dataclasses import dataclass, field, replace
|
||||
|
||||
@@ -170,9 +170,7 @@ def match_fw_to_car(fw_versions: list[CarParams.CarFw], vin: str, allow_exact: b
|
||||
return True, set()
|
||||
|
||||
|
||||
def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback,
|
||||
num_pandas: int = 1, skip_buses: set[int] | None = None) -> set[EcuAddrBusType]:
|
||||
skip_buses = skip_buses or set()
|
||||
def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]:
|
||||
# queries are split by OBD multiplexing mode
|
||||
queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []}
|
||||
parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []}
|
||||
@@ -180,7 +178,7 @@ def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_o
|
||||
|
||||
for brand, config, r in REQUESTS:
|
||||
# Skip query if no panda available
|
||||
if r.bus > num_pandas * 4 - 1 or r.bus in skip_buses:
|
||||
if r.bus > num_pandas * 4 - 1:
|
||||
continue
|
||||
|
||||
for ecu_type, addr, sub_addr in config.get_all_ecus(VERSIONS[brand]):
|
||||
@@ -237,8 +235,7 @@ def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, list[b
|
||||
|
||||
|
||||
def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, vin: str,
|
||||
ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1,
|
||||
progress: bool = False, skip_buses: set[int] | None = None) -> list[CarParams.CarFw]:
|
||||
ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]:
|
||||
"""Queries for FW versions ordering brands by likelihood, breaks when exact match is found"""
|
||||
|
||||
all_car_fw = []
|
||||
@@ -251,8 +248,7 @@ def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable
|
||||
if True not in brand_matches[brand]:
|
||||
continue
|
||||
|
||||
car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout,
|
||||
num_pandas=num_pandas, progress=progress, skip_buses=skip_buses)
|
||||
car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, progress=progress)
|
||||
all_car_fw.extend(car_fw)
|
||||
|
||||
# If there is a match using this brand's FW alone, finish querying early
|
||||
@@ -264,9 +260,7 @@ def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable
|
||||
|
||||
|
||||
def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str = None,
|
||||
extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, progress: bool = False,
|
||||
skip_buses: set[int] | None = None) -> list[CarParams.CarFw]:
|
||||
skip_buses = skip_buses or set()
|
||||
extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]:
|
||||
versions = VERSIONS.copy()
|
||||
|
||||
if query_brand is not None:
|
||||
@@ -304,7 +298,7 @@ def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_ob
|
||||
for addr_chunk in chunks(addr_group):
|
||||
for brand, config, r in requests:
|
||||
# Skip query if no panda available
|
||||
if r.bus > num_pandas * 4 - 1 or r.bus in skip_buses:
|
||||
if r.bus > num_pandas * 4 - 1:
|
||||
continue
|
||||
|
||||
# Toggle OBD multiplexing for each request
|
||||
|
||||
@@ -852,7 +852,6 @@ class CarController(CarControllerBase):
|
||||
CAR.CHEVROLET_VOLT_CC,
|
||||
CAR.CHEVROLET_MALIBU_CC,
|
||||
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
||||
CAR.BUICK_LACROSSE,
|
||||
}
|
||||
|
||||
if (self.CP.enableGasInterceptorDEPRECATED and self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
||||
|
||||
@@ -1,7 +1,4 @@
|
||||
import copy
|
||||
import math
|
||||
from datetime import UTC, datetime, timedelta
|
||||
from collections.abc import Mapping
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
@@ -40,140 +37,6 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
|
||||
HARD_BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise}
|
||||
NORMAL_CRUISE_BUTTONS = (CruiseButtons.RES_ACCEL, CruiseButtons.DECEL_SET)
|
||||
|
||||
# The CT6 DBC describes the PPS receiver output. These messages have been
|
||||
# observed on the GM powertrain bus at approximately 10 Hz; they remain an
|
||||
# optional parser so a vehicle which does not provide them is unaffected.
|
||||
PPS_GPS_MESSAGES = (
|
||||
"PPS_ElevHdSpd_FO",
|
||||
"PPS_PosLat_FO",
|
||||
"PPS_PosLong_FO",
|
||||
"PPS_Time_FO",
|
||||
"PPS_QualMetrics_FO",
|
||||
)
|
||||
# PPS_SigAcqTime_FO is intentionally not a health gate: on the observed GM
|
||||
# route its validity bit remains 1 even while the position bundle is valid.
|
||||
PPS_GPS_VALIDITY_SIGNALS = (
|
||||
"PPSLatV", "PPSLongV", "PPS2DAbsPosErrEstmtV", "PPSMdV", "PPSPstnDilPrcsV",
|
||||
"PPSTmdayV", "PPSCldrDayV", "PPSCldrYrV",
|
||||
)
|
||||
PPS_GPS_VALIDITY_MESSAGES = {
|
||||
"PPSLatV": "PPS_PosLat_FO",
|
||||
"PPSLongV": "PPS_PosLong_FO",
|
||||
"PPS2DAbsPosErrEstmtV": "PPS_QualMetrics_FO",
|
||||
"PPSMdV": "PPS_QualMetrics_FO",
|
||||
"PPSPstnDilPrcsV": "PPS_QualMetrics_FO",
|
||||
"PPSTmdayV": "PPS_Time_FO",
|
||||
"PPSCldrDayV": "PPS_Time_FO",
|
||||
"PPSCldrYrV": "PPS_Time_FO",
|
||||
}
|
||||
|
||||
|
||||
def pps_checksum_ok(data: bytes) -> bool:
|
||||
"""Validate the 11-bit checksum used by the observed PPS frames."""
|
||||
if len(data) < 2:
|
||||
return False
|
||||
received = ((data[-2] & 0x07) << 8) | data[-1]
|
||||
expected = sum(data[:-2]) + (data[-2] >> 3) + 0x4C
|
||||
return (expected & 0x7FF) == received
|
||||
|
||||
|
||||
def _invalid_pps_sample(timestamp_nanos: int) -> dict:
|
||||
return {
|
||||
"timestamp_nanos": timestamp_nanos,
|
||||
"latitude": 0.0,
|
||||
"longitude": 0.0,
|
||||
"altitude": 0.0,
|
||||
"speed": 0.0,
|
||||
"bearingDeg": 0.0,
|
||||
"horizontalAccuracy": 0.0,
|
||||
"unixTimestampMillis": 0,
|
||||
"verticalAccuracy": 0.0,
|
||||
"bearingAccuracyDeg": 180.0,
|
||||
"speedAccuracy": 0.0,
|
||||
"hasFix": False,
|
||||
"satelliteCount": 0,
|
||||
"vNED": [0.0, 0.0, 0.0],
|
||||
}
|
||||
|
||||
|
||||
def decode_gm_pps_gps(values: Mapping[str, Mapping[str, float]], raw: Mapping[str, bytes],
|
||||
timestamp_nanos: int) -> dict | None:
|
||||
"""Decode one coherent PPS bundle into the existing car-GPS sample shape."""
|
||||
if any(not pps_checksum_ok(raw.get(name, b"")) for name in PPS_GPS_MESSAGES):
|
||||
return None
|
||||
|
||||
try:
|
||||
if any(int(values[PPS_GPS_VALIDITY_MESSAGES[signal]].get(signal, 1)) != 0 for signal in PPS_GPS_VALIDITY_SIGNALS):
|
||||
return None
|
||||
# Mode 6 is explicitly "DR ONLY" in the DBC and is not an independent GPS
|
||||
# position. All other advertised modes include GNSS.
|
||||
if int(values["PPS_QualMetrics_FO"]["PPSMd"]) == 6:
|
||||
return None
|
||||
|
||||
latitude = float(values["PPS_PosLat_FO"]["PPSLat"])
|
||||
longitude = float(values["PPS_PosLong_FO"]["PPSLong"])
|
||||
if not (math.isfinite(latitude) and math.isfinite(longitude) and
|
||||
-90.0 <= latitude / 3_600_000.0 <= 90.0 and
|
||||
-180.0 <= longitude / 3_600_000.0 <= 180.0):
|
||||
return None
|
||||
latitude /= 3_600_000.0
|
||||
longitude /= 3_600_000.0
|
||||
if latitude == 0.0 and longitude == 0.0:
|
||||
return None
|
||||
|
||||
year = int(values["PPS_Time_FO"]["PPSCldrYr"])
|
||||
day_of_year = int(values["PPS_Time_FO"]["PPSCldrDay"])
|
||||
millis_of_day = int(values["PPS_Time_FO"]["PPSTmday"])
|
||||
if not 2014 <= year <= 2141 or not 0 <= day_of_year <= 365 or not 0 <= millis_of_day < 86_400_000:
|
||||
return None
|
||||
timestamp = datetime(year, 1, 1, tzinfo=UTC) + timedelta(days=day_of_year, milliseconds=millis_of_day)
|
||||
|
||||
speed = float(values["PPS_ElevHdSpd_FO"]["PPSVel"]) * CV.KPH_TO_MS
|
||||
heading = float(values["PPS_ElevHdSpd_FO"]["PPSHedng"])
|
||||
if int(values["PPS_ElevHdSpd_FO"].get("PPSVelV", 1)) != 0 or not math.isfinite(speed) or not 0.0 <= speed <= 200.0:
|
||||
speed = 0.0
|
||||
if (int(values["PPS_ElevHdSpd_FO"].get("PPSHedngV", 1)) != 0 or
|
||||
not math.isfinite(heading) or not 0.0 <= heading < 360.0):
|
||||
heading = 0.0
|
||||
|
||||
altitude = float(values["PPS_ElevHdSpd_FO"]["PPSElvtn"])
|
||||
if int(values["PPS_ElevHdSpd_FO"].get("PPSElvtnV", 1)) != 0 or not math.isfinite(altitude):
|
||||
altitude = 0.0
|
||||
else:
|
||||
altitude /= 100.0
|
||||
|
||||
horizontal_accuracy = float(values["PPS_QualMetrics_FO"]["PPS2DAbsPosErrEstmt"])
|
||||
vertical_accuracy = float(values["PPS_QualMetrics_FO"]["PPS3DAbsPosErrEstmt"])
|
||||
bearing_accuracy = float(values["PPS_QualMetrics_FO"]["PPSAbsHdngErrEstmt"])
|
||||
if not math.isfinite(horizontal_accuracy) or horizontal_accuracy < 0.0:
|
||||
horizontal_accuracy = 0.0
|
||||
if not math.isfinite(vertical_accuracy) or vertical_accuracy < 0.0:
|
||||
vertical_accuracy = 0.0
|
||||
if not math.isfinite(bearing_accuracy) or bearing_accuracy < 0.0:
|
||||
bearing_accuracy = 180.0
|
||||
except (KeyError, TypeError, ValueError, OverflowError):
|
||||
return None
|
||||
|
||||
heading_rad = math.radians(heading)
|
||||
return {
|
||||
"timestamp_nanos": timestamp_nanos,
|
||||
"latitude": latitude,
|
||||
"longitude": longitude,
|
||||
"altitude": altitude,
|
||||
"speed": speed,
|
||||
"bearingDeg": heading,
|
||||
"horizontalAccuracy": horizontal_accuracy,
|
||||
"unixTimestampMillis": int(timestamp.timestamp() * 1000),
|
||||
"verticalAccuracy": vertical_accuracy,
|
||||
"bearingAccuracyDeg": bearing_accuracy,
|
||||
# The CT6 DBC does not document the velocity-error units, so do not expose
|
||||
# a made-up conversion as a speed accuracy value.
|
||||
"speedAccuracy": 0.0,
|
||||
"hasFix": True,
|
||||
"satelliteCount": 0,
|
||||
"vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0],
|
||||
}
|
||||
|
||||
|
||||
def get_hard_cruise_buttons(steering_button_msg: dict) -> int:
|
||||
return steering_button_msg.get("ACCButtonsHard", CruiseButtons.INIT)
|
||||
@@ -245,16 +108,9 @@ class CarState(CarStateBase):
|
||||
self.car_gps_config = get_car_gps_config(CP)
|
||||
self.car_gps_supported = self.car_gps_config is not None
|
||||
self.car_gps = None
|
||||
self.onstar_gps = None
|
||||
self._car_gps_timestamp_nanos = 0
|
||||
self._prev_gps_lat = None
|
||||
self._prev_gps_lon = None
|
||||
self._last_gps_bearing = None
|
||||
|
||||
self.pps_gps = None
|
||||
self._pps_gps_timestamp_nanos = 0
|
||||
|
||||
def _update_car_gps(self, cp, v_ego: float = 0.0) -> None:
|
||||
def _update_car_gps(self, cp) -> None:
|
||||
if self.car_gps_config is None:
|
||||
return
|
||||
|
||||
@@ -269,66 +125,12 @@ class CarState(CarStateBase):
|
||||
gps = self.car_gps_config.decoder(*(cp.vl[name] for name in self.car_gps_config.messages))
|
||||
if gps is not None:
|
||||
gps["timestamp_nanos"] = timestamp_nanos
|
||||
if gps["hasFix"]:
|
||||
lat, lon = gps["latitude"], gps["longitude"]
|
||||
if self._prev_gps_lat is not None and (lat, lon) != (self._prev_gps_lat, self._prev_gps_lon):
|
||||
d_lat = (lat - self._prev_gps_lat) * 111139.0
|
||||
d_lon = (lon - self._prev_gps_lon) * 111139.0 * math.cos(math.radians(lat))
|
||||
if math.hypot(d_lat, d_lon) > 1.5 and v_ego > 1.0 and not self.moving_backward:
|
||||
self._last_gps_bearing = math.degrees(math.atan2(d_lon, d_lat)) % 360.0
|
||||
|
||||
self._prev_gps_lat, self._prev_gps_lon = lat, lon
|
||||
|
||||
bearing = self._last_gps_bearing if self._last_gps_bearing is not None else 0.0
|
||||
gps["speed"] = max(0.0, v_ego)
|
||||
gps["bearingDeg"] = bearing
|
||||
gps["bearingAccuracyDeg"] = 5.0 if (v_ego > 1.0 and self._last_gps_bearing is not None) else 180.0
|
||||
heading_rad = math.radians(bearing)
|
||||
gps["vNED"] = [v_ego * math.cos(heading_rad), v_ego * math.sin(heading_rad), 0.0]
|
||||
else:
|
||||
self._prev_gps_lat = self._prev_gps_lon = None
|
||||
|
||||
self.onstar_gps = gps
|
||||
self.car_gps = gps
|
||||
self._car_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
def _update_pps_gps(self, cp) -> None:
|
||||
"""Decode a complete, checksum-valid PPS burst when one is available."""
|
||||
timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in PPS_GPS_MESSAGES]
|
||||
if not all(timestamps):
|
||||
return
|
||||
|
||||
timestamp_nanos = max(timestamps)
|
||||
if timestamp_nanos <= self._pps_gps_timestamp_nanos:
|
||||
return
|
||||
if timestamp_nanos - min(timestamps) > 100_000_000:
|
||||
return
|
||||
|
||||
burst_ids = (
|
||||
int(cp.vl["PPS_ElevHdSpd_FO"]["PPSElvHedngSpdBrstID"]),
|
||||
int(cp.vl["PPS_PosLat_FO"]["PPSLatBrstID"]),
|
||||
int(cp.vl["PPS_PosLong_FO"]["PPSLongBrstID"]),
|
||||
int(cp.vl["PPS_Time_FO"]["PPSTmBrstID"]),
|
||||
int(cp.vl["PPS_QualMetrics_FO"]["PPSPosQltyMtcBrstID"]),
|
||||
)
|
||||
if len(set(burst_ids)) != 1:
|
||||
return
|
||||
|
||||
values = {name: cp.vl[name] for name in PPS_GPS_MESSAGES}
|
||||
raw = {name: cp.vl_raw[name] for name in PPS_GPS_MESSAGES}
|
||||
gps = decode_gm_pps_gps(values, raw, timestamp_nanos)
|
||||
self.pps_gps = gps if gps is not None else _invalid_pps_sample(timestamp_nanos)
|
||||
self._pps_gps_timestamp_nanos = timestamp_nanos
|
||||
|
||||
def get_car_gps(self):
|
||||
return self.car_gps
|
||||
|
||||
def get_car_gps_sources(self):
|
||||
return {
|
||||
"pps": self.pps_gps,
|
||||
"onstar": self.onstar_gps,
|
||||
}
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
for b in buttonEvents:
|
||||
@@ -343,6 +145,8 @@ class CarState(CarStateBase):
|
||||
cam_cp = can_parsers[Bus.cam]
|
||||
loopback_cp = can_parsers[Bus.loopback]
|
||||
|
||||
self._update_car_gps(pt_cp)
|
||||
|
||||
ret = structs.CarState()
|
||||
|
||||
volt_like = {
|
||||
@@ -413,11 +217,6 @@ class CarState(CarStateBase):
|
||||
ret.standstill = abs(pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"]) <= STANDSTILL_THRESHOLD and \
|
||||
abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD
|
||||
|
||||
self._update_car_gps(pt_cp, ret.vEgo)
|
||||
pps_cp = can_parsers.get(Bus.adas)
|
||||
if pps_cp is not None:
|
||||
self._update_pps_gps(pps_cp)
|
||||
|
||||
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
|
||||
ret.gearShifter = self.parse_gear_shifter("T")
|
||||
else:
|
||||
@@ -766,18 +565,8 @@ class CarState(CarStateBase):
|
||||
("ASCMLKASteeringCmd", 0),
|
||||
]
|
||||
|
||||
parsers = {
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus.POWERTRAIN),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus.CAMERA),
|
||||
Bus.loopback: CANParser(DBC[CP.carFingerprint][Bus.pt], loopback_messages, CanBus.LOOPBACK),
|
||||
}
|
||||
if getattr(CP, "brand", None) == "gm":
|
||||
# Keep the CT6 object DBC isolated from the active vehicle DBC. Every
|
||||
# signal is optional, so a GM vehicle without PPS frames stays can-valid
|
||||
# and follows the existing device/OnStar behavior.
|
||||
parsers[Bus.adas] = CANParser(
|
||||
"cadillac_ct6_object",
|
||||
[(name, 0) for name in PPS_GPS_MESSAGES],
|
||||
CanBus.POWERTRAIN,
|
||||
)
|
||||
return parsers
|
||||
|
||||
@@ -28,9 +28,6 @@ BOLT_CC_BUTTON_CARS = {
|
||||
BOLT_CC_TARGET_DEADBAND_MPH = 0.75
|
||||
BOLT_CC_REVERSE_CONFIRM_S = 0.6
|
||||
BOLT_CC_DIRECTION_MEMORY_S = 1.5
|
||||
VOLT_CC_CARS = {
|
||||
CAR.CHEVROLET_VOLT_CC,
|
||||
}
|
||||
|
||||
|
||||
def malibu_phase_map_for_button(button):
|
||||
@@ -339,28 +336,6 @@ def stabilize_bolt_cc_button(controller, CP, requested_button):
|
||||
return requested_button
|
||||
|
||||
|
||||
def _create_volt_cc_spam_command(CS, actuators, ms_convert):
|
||||
accel = float(actuators.accel)
|
||||
speed_setpoint = int(round(CS.out.cruiseState.speed * ms_convert))
|
||||
ego_speed = CS.out.vEgo * ms_convert
|
||||
|
||||
if accel == 0.0:
|
||||
return CruiseButtons.INIT, float("inf")
|
||||
|
||||
if accel < 0.0:
|
||||
if speed_setpoint > ego_speed + 3.0:
|
||||
rate = 0.2
|
||||
else:
|
||||
rate = max(1.0 / (-accel * ms_convert), 0.2)
|
||||
return CruiseButtons.DECEL_SET, rate
|
||||
|
||||
if speed_setpoint < ego_speed - 3.0:
|
||||
rate = 0.2
|
||||
else:
|
||||
rate = max(1.0 / (accel * ms_convert), 0.2)
|
||||
return CruiseButtons.RES_ACCEL, rate
|
||||
|
||||
|
||||
def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggles):
|
||||
accel = actuators.accel
|
||||
v_ego = CS.out.vEgo
|
||||
@@ -375,15 +350,12 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators, starpilot_toggl
|
||||
target_deadband = BOLT_CC_TARGET_DEADBAND_MPH * (CV.MPH_TO_KPH if is_metric else 1.0) if bolt_cc else 0.0
|
||||
comparison_setpoint = projected_setpoint if bolt_cc else desired_setpoint
|
||||
|
||||
if CS.CP.carFingerprint in VOLT_CC_CARS:
|
||||
cruise_btn, rate = _create_volt_cc_spam_command(CS, actuators, ms_convert)
|
||||
else:
|
||||
if CS.CP.minEnableSpeed - (desired_setpoint / ms_convert) > 3.25:
|
||||
cruise_btn = CruiseButtons.CANCEL
|
||||
elif comparison_setpoint < speed_setpoint - target_deadband and speed_setpoint > CS.CP.minEnableSpeed * ms_convert + 1:
|
||||
cruise_btn = CruiseButtons.DECEL_SET
|
||||
elif comparison_setpoint > speed_setpoint + target_deadband:
|
||||
cruise_btn = CruiseButtons.RES_ACCEL
|
||||
if CS.CP.minEnableSpeed - (desired_setpoint / ms_convert) > 3.25:
|
||||
cruise_btn = CruiseButtons.CANCEL
|
||||
elif comparison_setpoint < speed_setpoint - target_deadband and speed_setpoint > CS.CP.minEnableSpeed * ms_convert + 1:
|
||||
cruise_btn = CruiseButtons.DECEL_SET
|
||||
elif comparison_setpoint > speed_setpoint + target_deadband:
|
||||
cruise_btn = CruiseButtons.RES_ACCEL
|
||||
|
||||
cruise_btn = stabilize_bolt_cc_button(controller, CS.CP, cruise_btn)
|
||||
if cruise_btn == CruiseButtons.CANCEL:
|
||||
|
||||
@@ -8,14 +8,7 @@ from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, DT_CTRL, structs
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.gm import gmcan
|
||||
from opendbc.car.gm.carstate import (
|
||||
CarState as GMCarState,
|
||||
PPS_GPS_MESSAGES,
|
||||
decode_gm_pps_gps,
|
||||
get_hard_cruise_buttons,
|
||||
pps_checksum_ok,
|
||||
update_auto_hold_drive_timers,
|
||||
)
|
||||
from opendbc.car.gm.carstate import CarState as GMCarState, get_hard_cruise_buttons, update_auto_hold_drive_timers
|
||||
from opendbc.car.gm.carcontroller import (
|
||||
VisualAlert,
|
||||
get_acc_dashboard_always_one,
|
||||
@@ -95,145 +88,6 @@ class TestBoltGps:
|
||||
assert gps["latitude"] == 0.0
|
||||
assert gps["longitude"] == 0.0
|
||||
|
||||
def test_bolt_gps_accuracy_metrics(self):
|
||||
gps = parse_chevrolet_bolt_can_gps({"GPSLatitude": 145292743.0, "GPSLongitude": -267520892.0})
|
||||
assert gps is not None
|
||||
assert gps["horizontalAccuracy"] == 6.0
|
||||
assert gps["verticalAccuracy"] == 10.0
|
||||
assert gps["speedAccuracy"] == 0.5
|
||||
|
||||
|
||||
class TestPpsGps:
|
||||
_frames = [
|
||||
(0x260, bytes.fromhex("10ddac000d831277"), 0),
|
||||
(0x261, bytes.fromhex("08386fce09ca"), 0),
|
||||
(0x262, bytes.fromhex("6d98820341de"), 0),
|
||||
(0x264, bytes.fromhex("0018f90578b5eaac"), 0),
|
||||
(0x265, bytes.fromhex("1a0000800a0258fd"), 0),
|
||||
]
|
||||
|
||||
def test_observed_bundle_checksum_and_conversion(self):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, self._frames)])
|
||||
|
||||
assert all(pps_checksum_ok(parser.vl_raw[name]) for name in PPS_GPS_MESSAGES)
|
||||
gps = decode_gm_pps_gps(
|
||||
{name: parser.vl[name] for name in PPS_GPS_MESSAGES},
|
||||
{name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES},
|
||||
1_000_000_000,
|
||||
)
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
assert gps["latitude"] == pytest.approx(38.3101, abs=1e-4)
|
||||
assert gps["longitude"] == pytest.approx(-85.7701, abs=1e-4)
|
||||
assert gps["unixTimestampMillis"] == 1788742068655
|
||||
|
||||
def test_invalid_position_validity_is_rejected(self):
|
||||
parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
|
||||
parser.update([(1_000_000_000, self._frames)])
|
||||
values = {name: dict(parser.vl[name]) for name in PPS_GPS_MESSAGES}
|
||||
values["PPS_PosLat_FO"]["PPSLatV"] = 1
|
||||
gps = decode_gm_pps_gps(
|
||||
values,
|
||||
{name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES},
|
||||
1_000_000_000,
|
||||
)
|
||||
assert gps is None
|
||||
|
||||
def test_bolt_gps_heading_and_speed_derivation(self):
|
||||
cp = SimpleNamespace(
|
||||
brand="gm",
|
||||
carFingerprint=CAR.CHEVROLET_BOLT_CC_2018_2021,
|
||||
flags=0,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
transmissionType=structs.CarParams.TransmissionType.direct,
|
||||
enableBsm=False,
|
||||
enableGasInterceptorDEPRECATED=False,
|
||||
pcmCruise=False,
|
||||
)
|
||||
fpcp = custom.StarPilotCarParams.new_message()
|
||||
cs = GMCarState(cp, fpcp)
|
||||
|
||||
# First position (stationary)
|
||||
mock_cp = SimpleNamespace(
|
||||
ts_nanos={"TCICOnStarGPSPosition": {"GPSLatitude": 1_000_000}},
|
||||
vl={"TCICOnStarGPSPosition": {"GPSLatitude": 145292743.0, "GPSLongitude": -267520892.0}},
|
||||
)
|
||||
cs._update_car_gps(mock_cp, v_ego=0.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["speed"] == 0.0
|
||||
assert gps["bearingDeg"] == 0.0
|
||||
assert gps["bearingAccuracyDeg"] == 180.0
|
||||
|
||||
# Move East at 15 m/s
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 2_000_000_000
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = -267520892.0 + 1000.0 # Eastward shift
|
||||
cs._update_car_gps(mock_cp, v_ego=15.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["speed"] == 15.0
|
||||
assert gps["bearingDeg"] == pytest.approx(90.0, abs=1.0)
|
||||
assert gps["bearingAccuracyDeg"] == 5.0
|
||||
assert gps["vNED"][1] > 0.0 # East velocity positive
|
||||
|
||||
# Stop moving (v_ego=0.0): heading should be retained, not reset to 0
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 3_000_000_000
|
||||
cs._update_car_gps(mock_cp, v_ego=0.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["speed"] == 0.0
|
||||
assert gps["bearingDeg"] == pytest.approx(90.0, abs=1.0)
|
||||
|
||||
# Reversing: coordinate changes while moving backward should not flip heading
|
||||
cs.moving_backward = True
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 4_000_000_000
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = -267520892.0 - 1000.0 # Westward shift
|
||||
cs._update_car_gps(mock_cp, v_ego=3.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["bearingDeg"] == pytest.approx(90.0, abs=1.0)
|
||||
|
||||
# Drive True North: verify bearing is 0.0 deg and accuracy is 5.0 deg (not degraded to 180.0)
|
||||
cs.moving_backward = False
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 5_000_000_000
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 145292743.0 + 1000.0 # Northward shift
|
||||
cs._update_car_gps(mock_cp, v_ego=12.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["bearingDeg"] == pytest.approx(0.0, abs=1.0)
|
||||
assert gps["bearingAccuracyDeg"] == 5.0
|
||||
|
||||
# Tunnel / fix loss: invalid coordinates cause hasFix=False and clear previous coordinates
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 6_000_000_000
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 0.0
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = 0.0
|
||||
cs._update_car_gps(mock_cp, v_ego=20.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert not gps["hasFix"]
|
||||
assert cs._prev_gps_lat is None and cs._prev_gps_lon is None
|
||||
|
||||
# Tunnel exit: GPS fix re-acquired 5 km away heading South
|
||||
# The first sample after fix loss sets initial coordinates without calculating a phantom jump vector
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 7_000_000_000
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 145292743.0 - 50000.0
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLongitude"] = -267520892.0
|
||||
cs._update_car_gps(mock_cp, v_ego=20.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["hasFix"]
|
||||
assert gps["bearingDeg"] == pytest.approx(0.0, abs=1.0)
|
||||
assert cs._prev_gps_lat is not None
|
||||
|
||||
# Second sample: moving Southward -> bearing smoothly updates to 180 deg
|
||||
mock_cp.ts_nanos["TCICOnStarGPSPosition"]["GPSLatitude"] = 8_000_000_000
|
||||
mock_cp.vl["TCICOnStarGPSPosition"]["GPSLatitude"] = 145292743.0 - 51000.0
|
||||
cs._update_car_gps(mock_cp, v_ego=20.0)
|
||||
gps = cs.get_car_gps()
|
||||
assert gps is not None
|
||||
assert gps["bearingDeg"] == pytest.approx(180.0, abs=1.0)
|
||||
|
||||
@parameterized.expand(CHEVROLET_BOLT_GPS_CARS)
|
||||
def test_gps_message_is_added_to_powertrain_parser(self, car_model):
|
||||
cp = SimpleNamespace(
|
||||
@@ -803,60 +657,6 @@ class TestGMCarController:
|
||||
|
||||
assert [msg[2] for msg in msgs] == [0, 2]
|
||||
|
||||
def test_volt_cc_redneck_holds_setpoint_without_planner_acceleration(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(2.0 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=60.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=60.0 * CV.KPH_TO_MS),
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.0), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
assert controller.apply_speed == 60
|
||||
|
||||
def test_volt_cc_redneck_rate_limits_setpoint_changes_by_planner_acceleration(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.5 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
cs = SimpleNamespace(
|
||||
CP=SimpleNamespace(
|
||||
carFingerprint=CAR.CHEVROLET_VOLT_CC,
|
||||
flags=GMFlags.NO_CAMERA.value,
|
||||
networkLocation=structs.CarParams.NetworkLocation.gateway,
|
||||
minEnableSpeed=0.0,
|
||||
),
|
||||
buttons_counter=2,
|
||||
out=SimpleNamespace(
|
||||
vEgo=60.0 * CV.KPH_TO_MS,
|
||||
cruiseState=SimpleNamespace(speed=60.0 * CV.KPH_TO_MS),
|
||||
),
|
||||
)
|
||||
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert msgs == []
|
||||
|
||||
controller.frame = int(0.7 / DT_CTRL)
|
||||
msgs = gmcan.create_gm_cc_spam_command(
|
||||
packer, controller, cs, SimpleNamespace(accel=0.5), SimpleNamespace(is_metric=True),
|
||||
)
|
||||
|
||||
assert len(msgs) == 1
|
||||
|
||||
def test_volt_cc_no_camera_redneck_spam_stays_on_powertrain_bus(self):
|
||||
packer = CANPacker(DBC[CAR.CHEVROLET_VOLT_CC][Bus.pt])
|
||||
controller = SimpleNamespace(frame=int(0.3 / DT_CTRL), last_button_frame=0, apply_speed=0, malibu_button_phase=0)
|
||||
|
||||
@@ -3,14 +3,11 @@ import math
|
||||
from dataclasses import dataclass
|
||||
from datetime import UTC, datetime
|
||||
from collections.abc import Callable, Mapping
|
||||
from functools import cache
|
||||
from typing import Any
|
||||
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.can.dbc import DBC as DBC_FILE
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.ford.values import CAR as FORD_CAR
|
||||
from opendbc.car.gm.values import CAR as GM_CAR, DBC as GM_DBC
|
||||
from opendbc.car.gm.values import CAR as GM_CAR
|
||||
|
||||
|
||||
CarGpsSample = dict[str, Any]
|
||||
@@ -116,11 +113,11 @@ def parse_chevrolet_bolt_can_gps(position: Mapping[str, float]) -> CarGpsSample
|
||||
"altitude": 0.0,
|
||||
"speed": 0.0,
|
||||
"bearingDeg": 0.0,
|
||||
"horizontalAccuracy": 6.0,
|
||||
"horizontalAccuracy": 100.0,
|
||||
"unixTimestampMillis": int(datetime.now(UTC).timestamp() * 1000),
|
||||
"verticalAccuracy": 10.0,
|
||||
"verticalAccuracy": 100.0,
|
||||
"bearingAccuracyDeg": 180.0,
|
||||
"speedAccuracy": 0.5,
|
||||
"speedAccuracy": 100.0,
|
||||
"hasFix": coordinates_valid,
|
||||
"satelliteCount": 0,
|
||||
"vNED": [0.0, 0.0, 0.0],
|
||||
@@ -160,33 +157,9 @@ CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
|
||||
}
|
||||
|
||||
|
||||
@cache
|
||||
def _gm_dbc_has_onstar_gps(dbc_name: str) -> bool:
|
||||
return "TCICOnStarGPSPosition" in DBC_FILE(dbc_name).name_to_msg
|
||||
|
||||
|
||||
def get_car_gps_config(CP) -> CarGpsConfig | None:
|
||||
cp_brand = getattr(CP, "brand", None)
|
||||
config = CAR_GPS_CONFIGS.get(CP.carFingerprint)
|
||||
if config is not None and config.brand == cp_brand:
|
||||
return config
|
||||
|
||||
# The OnStar position message is shared by the GM powertrain DBCs. Keep the
|
||||
# existing explicit map for known vehicles, but enable the same decoder for
|
||||
# other GM fingerprints only when their active DBC actually defines it.
|
||||
if cp_brand == "gm":
|
||||
try:
|
||||
dbc_name = GM_DBC[CP.carFingerprint][Bus.pt]
|
||||
if _gm_dbc_has_onstar_gps(dbc_name):
|
||||
return CarGpsConfig(
|
||||
brand="gm",
|
||||
messages=CHEVROLET_BOLT_GPS_MESSAGES,
|
||||
decoder=parse_chevrolet_bolt_can_gps,
|
||||
)
|
||||
except (KeyError, OSError, TypeError, RuntimeError):
|
||||
pass
|
||||
|
||||
return None
|
||||
return config if config is not None and config.brand == CP.brand else None
|
||||
|
||||
|
||||
def car_gps_available(CP) -> bool:
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
from dataclasses import dataclass
|
||||
|
||||
# Provenance: portions of HKG angle control are adapted from sunnypilot/opendbc's
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
|
||||
@@ -10,7 +8,7 @@ from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_an
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.hyundai import hyundaicanfd, hyundaican
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiSafetyFlags, HyundaiStarPilotFlags, Buttons, CarControllerParams, CAR, CANFD_ANGLE_LONGITUDINAL_CAR, \
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiStarPilotFlags, Buttons, CarControllerParams, CAR, CANFD_ANGLE_LONGITUDINAL_CAR, \
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR, CANFD_ALT_BUTTONS_RESUME_CAR, kia_ev6_gt_line_longitudinal_tuning, \
|
||||
KIA_EV6_GT_LINE_LONG_TUNING_TESTING_GROUND_ID
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
@@ -26,9 +24,6 @@ LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
MAX_ANGLE = 85
|
||||
MAX_ANGLE_FRAMES = 89
|
||||
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
|
||||
|
||||
CANCEL_BUTTON_DELAY_FRAMES = 10
|
||||
|
||||
CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000
|
||||
CANFD_CAMERA_LEAD_STALE_NS = 300_000_000
|
||||
CANFD_LEAD_MIN_DISTANCE = 0.1
|
||||
@@ -459,7 +454,6 @@ class CarController(CarControllerBase):
|
||||
self.apply_angle_last = 0.0
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.last_button_frame = 0
|
||||
self.cancel_counter = 0
|
||||
self.redneck_button_frame = 0
|
||||
self.ecu_disable_failed = False
|
||||
self._ecu_disable_checked = False
|
||||
@@ -477,11 +471,6 @@ class CarController(CarControllerBase):
|
||||
self._dash_lat_disengage_init = False
|
||||
self._dash_prev_lat_active = False
|
||||
self._ray_lkas11_active = False
|
||||
self._ray_lfa_8byte = CP.carFingerprint == CAR.KIA_RAY_EV and bool(
|
||||
getattr(CP, "safetyConfigs", None) and
|
||||
CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS
|
||||
)
|
||||
self._ray_lfa_packer = CANPacker("hyundai_kia_ray_lfa") if self._ray_lfa_8byte else None
|
||||
|
||||
def _update_dash_icon_state(self, CC):
|
||||
if CC.latActive:
|
||||
@@ -728,8 +717,6 @@ class CarController(CarControllerBase):
|
||||
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
|
||||
can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True))
|
||||
|
||||
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
|
||||
|
||||
# *** CAN/CAN FD specific ***
|
||||
if self.CP.flags & HyundaiFlags.CANFD:
|
||||
can_sends.extend(self.create_canfd_msgs(now_nanos, apply_steer_req, apply_torque, apply_angle, set_speed_in_units, accel,
|
||||
@@ -755,7 +742,6 @@ class CarController(CarControllerBase):
|
||||
can_sends = []
|
||||
can_canfd_blended = bool(self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED)
|
||||
blended_hda2 = can_canfd_blended and bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING)
|
||||
longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False))
|
||||
|
||||
# HUD messages
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
|
||||
@@ -764,7 +750,6 @@ class CarController(CarControllerBase):
|
||||
if blended_hda2:
|
||||
can_sends.extend(hyundaicanfd.create_steering_messages(
|
||||
self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, 0.0,
|
||||
longitudinal_active=longitudinal_active,
|
||||
))
|
||||
if self.long_active_ecu:
|
||||
can_sends.extend(hyundaican.create_lkas11_can_canfd_blended(
|
||||
@@ -797,7 +782,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
# Button messages
|
||||
if not self.long_active_ecu:
|
||||
if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
|
||||
if CC.cruiseControl.cancel:
|
||||
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
|
||||
elif CC.cruiseControl.resume:
|
||||
# send resume at a max freq of 10Hz
|
||||
@@ -839,10 +824,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.SEND_LFA.value or (self.long_active_ecu and blended_hda2)):
|
||||
if self._ray_lfa_8byte:
|
||||
can_sends.append(hyundaican.create_ray_lfahda_mfc(self._ray_lfa_packer, CC.latActive, lfa_icon))
|
||||
else:
|
||||
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.frame, self.CP, lfa_icon))
|
||||
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.frame, self.CP, lfa_icon))
|
||||
|
||||
# 5 Hz ACC options
|
||||
if self.frame % 20 == 0 and self.long_active_ecu and not can_canfd_blended:
|
||||
@@ -859,9 +841,7 @@ class CarController(CarControllerBase):
|
||||
can_sends = []
|
||||
|
||||
lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
longitudinal_active = bool(self.long_active_ecu and getattr(CC, "longActive", False))
|
||||
lfa_longitudinal_active = longitudinal_active if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN else self.CP.openpilotLongitudinalControl
|
||||
lka_steering_long = lka_steering and lfa_longitudinal_active
|
||||
lka_steering_long = lka_steering and self.long_active_ecu
|
||||
ccnc_non_hda2 = self.CP.flags & HyundaiFlags.CCNC and not lka_steering
|
||||
use_egmp_dynamic_long_tuning = egmp_dynamic_longitudinal_tuning(self.CP) and self.long_active_ecu and \
|
||||
CC.actuators.longControlState in (LongCtrlState.starting, LongCtrlState.pid, LongCtrlState.stopping)
|
||||
@@ -890,7 +870,7 @@ class CarController(CarControllerBase):
|
||||
if angle_lkas_alt:
|
||||
steering_msg_active = bool(steering_msg_active and drive_gear)
|
||||
angle_lkas_alt_standstill_handoff = bool(getattr(CS.out, "standstill", False) and not CC.latActive)
|
||||
forward_stock_lkas = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and angle_lkas_alt and (
|
||||
forward_stock_lkas = angle_lkas_alt and (
|
||||
angle_lkas_alt_standstill_handoff or not (drive_gear and (CC.latActive or CC.enabled))
|
||||
)
|
||||
preserve_stock_lfa_status = preserve_stock_canfd_lfa_status(self.CP.carFingerprint)
|
||||
@@ -899,8 +879,7 @@ class CarController(CarControllerBase):
|
||||
steering_msg_active, apply_torque, apply_angle,
|
||||
CS.stock_lfa_msg if preserve_stock_lfa_status else None,
|
||||
CS.stock_lkas_msg if preserve_stock_lkas else None,
|
||||
lka_icon=lka_icon,
|
||||
longitudinal_active=lfa_longitudinal_active))
|
||||
lka_icon=lka_icon))
|
||||
direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault
|
||||
inactive_steering_angle = float(np.clip(CS.angle_steering_angle,
|
||||
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
|
||||
@@ -1067,7 +1046,7 @@ class CarController(CarControllerBase):
|
||||
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
|
||||
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
|
||||
self.last_button_frame = self.frame
|
||||
elif self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES:
|
||||
else:
|
||||
for _ in range(20):
|
||||
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter + 1, Buttons.CANCEL))
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
@@ -2,8 +2,6 @@ from collections import deque
|
||||
import copy
|
||||
import math
|
||||
|
||||
# Provenance: portions of HKG angle-state integration are adapted from sunnypilot/opendbc's
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
@@ -247,10 +245,7 @@ class CarState(CarStateBase):
|
||||
return button_events
|
||||
|
||||
def create_lkas_button_events(self, cp: CANParser, prev_lda_button: int) -> list[structs.CarState.ButtonEvent]:
|
||||
if self.CP.carFingerprint == CAR.KIA_RAY_EV:
|
||||
self.lda_button = int(cp.vl["BCM_PO_11"]["RAY_LKAS_BTN"] != 0) \
|
||||
if cp.ts_nanos["BCM_PO_11"]["RAY_LKAS_BTN"] > 0 else 0
|
||||
elif self.CP.carFingerprint == CAR.HYUNDAI_SONATA:
|
||||
if self.CP.carFingerprint == CAR.HYUNDAI_SONATA:
|
||||
self.lda_button = int(cp.vl["BCM_PO_11"]["LDA_BTN"]) if cp.ts_nanos["BCM_PO_11"]["LDA_BTN"] > 0 else 0
|
||||
elif self.CP.carFingerprint == CAR.HYUNDAI_SONATA_HYBRID:
|
||||
self.lda_button = self.get_sonata_hybrid_lkas_button_state(cp)
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
# Provenance: portions of HKG firmware data are adapted from sunnypilot/opendbc master at
|
||||
# f95f996f5 and its hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md.
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
|
||||
|
||||
@@ -203,17 +203,6 @@ def create_lfahda_mfc(packer, enabled, frame=None, CP=None, lfa_icon=None):
|
||||
return packer.make_can_msg("LFAHDA_MFC", bus, values)
|
||||
|
||||
|
||||
def create_ray_lfahda_mfc(packer, lat_active, lfa_icon):
|
||||
values = {
|
||||
"HDA_USM": 2,
|
||||
"HDA_Icon_State": 2 if lfa_icon else 0,
|
||||
"HDA_VSetReq": 0,
|
||||
"HDA_Icon_Wheel": int(lat_active),
|
||||
"LFA_Icon_State": lfa_icon,
|
||||
}
|
||||
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
||||
|
||||
|
||||
def create_acc_commands_can_canfd_blended(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed,
|
||||
stopping, long_override, use_fca, CP):
|
||||
commands = []
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
import copy
|
||||
# Provenance: portions of HKG angle-command construction are adapted from sunnypilot/opendbc's
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
import numpy as np
|
||||
from opendbc.car import CanBusBase, CanData
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
@@ -99,12 +97,9 @@ def create_angle_adas_cmd(packer, CAN, apply_angle: float, lat_active: bool, tor
|
||||
|
||||
|
||||
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle,
|
||||
lfa_base_values=None, lkas_base_values=None, lka_icon=None,
|
||||
longitudinal_active=None):
|
||||
lfa_base_values=None, lkas_base_values=None, lka_icon=None):
|
||||
if lka_icon is None:
|
||||
lka_icon = 2 if enabled else 1
|
||||
if longitudinal_active is None:
|
||||
longitudinal_active = CP.openpilotLongitudinalControl
|
||||
|
||||
angle_lkas_alt = CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT
|
||||
|
||||
@@ -198,7 +193,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
|
||||
ret = []
|
||||
if CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
|
||||
lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS"
|
||||
if longitudinal_active and not CP.flags & HyundaiFlags.CAN_CANFD_BLENDED:
|
||||
if CP.openpilotLongitudinalControl and not CP.flags & HyundaiFlags.CAN_CANFD_BLENDED:
|
||||
ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
|
||||
ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, lkas_values))
|
||||
else:
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
import time
|
||||
# Provenance: portions of HKG angle integration are adapted from sunnypilot/opendbc's
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
from opendbc.car import get_safety_config, structs, uds
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \
|
||||
@@ -50,7 +48,7 @@ def apply_platform_longitudinal_params(ret: structs.CarParams) -> None:
|
||||
|
||||
def apply_kia_ev6_gt_line_longitudinal_params(ret: structs.CarParams) -> None:
|
||||
ret.startAccel = 1.4
|
||||
ret.longitudinalActuatorDelay = 0.5
|
||||
ret.longitudinalActuatorDelay = 0.35
|
||||
ret.vEgoStarting = 0.5
|
||||
|
||||
|
||||
@@ -211,7 +209,9 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.enableBsm = 0x58b in fingerprint[CAN.ECAN]
|
||||
|
||||
# Send LFA message on cars with HDA
|
||||
if 0x485 in fingerprint[CAN.CAM]:
|
||||
if 0x485 in fingerprint[CAN.CAM] and (
|
||||
candidate != CAR.KIA_RAY_EV or fingerprint[CAN.CAM][0x485] == 4
|
||||
):
|
||||
ret.flags |= HyundaiFlags.SEND_LFA.value
|
||||
|
||||
# These cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
|
||||
@@ -226,9 +226,6 @@ class CarInterface(CarInterfaceBase):
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundai, 0)]
|
||||
|
||||
if candidate == CAR.KIA_RAY_EV and fingerprint[CAN.CAM].get(0x485) == 8:
|
||||
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAN_REFRESH_MSGS.value
|
||||
|
||||
if ret.flags & HyundaiFlags.CAMERA_SCC:
|
||||
ret.safetyConfigs[0].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value
|
||||
if candidate in (CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024):
|
||||
|
||||
@@ -7,7 +7,7 @@ from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
|
||||
from opendbc.car.structs import CarControl, CarParams
|
||||
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
|
||||
from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY_FRAMES, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \
|
||||
from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \
|
||||
EV9LongitudinalTuningState, update_ev9_longitudinal_tuning, \
|
||||
BlindspotWarningState, update_blindspot_warning, \
|
||||
reset_egmp_longitudinal_tuning, \
|
||||
@@ -783,36 +783,6 @@ class TestHyundaiFingerprint:
|
||||
assert not any(addr == 0x340 for addr, _, _ in first)
|
||||
assert any(addr == 0x340 for addr, _, _ in second)
|
||||
|
||||
def test_stock_scc_cancel_waits_for_factory_disengagement(self):
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_SANTA_FE_2022, gen_empty_fingerprint(), [], False, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0)
|
||||
|
||||
hud_control = SimpleNamespace(
|
||||
visualAlert=CarControl.HUDControl.VisualAlert.none,
|
||||
leftLaneVisible=True,
|
||||
rightLaneVisible=True,
|
||||
leftLaneDepart=False,
|
||||
rightLaneDepart=False,
|
||||
)
|
||||
CS = SimpleNamespace(
|
||||
lkas11=parser.vl["LKAS11"],
|
||||
clu11=parser.vl["CLU11"],
|
||||
redneck_send_button=Buttons.NONE,
|
||||
is_metric=False,
|
||||
)
|
||||
CC = SimpleNamespace(enabled=False, cruiseControl=SimpleNamespace(cancel=True, resume=False))
|
||||
actuators = SimpleNamespace(longControlState=LongCtrlState.off)
|
||||
|
||||
for counter in range(1, CANCEL_BUTTON_DELAY_FRAMES + 1):
|
||||
controller.cancel_counter = counter
|
||||
msgs = controller.create_can_msgs(True, 0, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 2, 2)
|
||||
assert not any(addr == 0x4F1 for addr, _, _ in msgs)
|
||||
|
||||
controller.cancel_counter = CANCEL_BUTTON_DELAY_FRAMES + 1
|
||||
msgs = controller.create_can_msgs(True, 0, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 2, 2)
|
||||
assert any(addr == 0x4F1 for addr, _, _ in msgs)
|
||||
|
||||
@pytest.mark.parametrize("candidate", (CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024))
|
||||
def test_hyundai_can_refresh_platforms_use_refresh_dbc_and_safety_param(self, candidate):
|
||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None)
|
||||
@@ -901,21 +871,7 @@ class TestHyundaiFingerprint:
|
||||
fingerprint[2][0x485] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, fingerprint, [], False, False, False, None)
|
||||
|
||||
assert CP.flags & HyundaiFlags.SEND_LFA
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS
|
||||
|
||||
def test_ray_ev_uses_carrot_eight_byte_lfa_frame(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[2][0x485] = 8
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, fingerprint, [], False, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
|
||||
msg = hyundaican.create_ray_lfahda_mfc(controller._ray_lfa_packer, True, 2)
|
||||
assert msg[0] == 0x485
|
||||
assert len(msg[1]) == 8
|
||||
assert msg[1][0] & 0x03 == 2
|
||||
assert msg[1][2] & 0x10 == 0x10
|
||||
assert msg[1][3] & 0x03 == 2
|
||||
assert not (CP.flags & HyundaiFlags.SEND_LFA)
|
||||
|
||||
def test_non_ray_legacy_platform_keeps_53e_lkas12_detection(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
@@ -1032,49 +988,6 @@ class TestHyundaiFingerprint:
|
||||
assert ret.cruiseState.enabled
|
||||
assert ret.cruiseState.speed == pytest.approx(10 * 0.2777778)
|
||||
|
||||
def test_kia_ray_ev_decodes_bcm_lkas_button_pulse(self):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(CAR.KIA_RAY_EV, gen_empty_fingerprint(), [], True, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.KIA_RAY_EV, gen_empty_fingerprint(), [], CP, toggles)
|
||||
car_state = CarState(CP, FPCP)
|
||||
can_parsers = car_state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
def update(ray_lkas_button: int, frame: int):
|
||||
msg = packer.make_can_msg("BCM_PO_11", 0, {"RAY_LKAS_BTN": ray_lkas_button})
|
||||
can_parsers[Bus.pt].update([(frame, [msg])])
|
||||
return car_state.update(can_parsers, toggles)[0]
|
||||
|
||||
update(0, 1)
|
||||
ret = update(1, 2)
|
||||
assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents)
|
||||
|
||||
ret = update(0, 3)
|
||||
assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents)
|
||||
|
||||
ret = update(1, 4)
|
||||
assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents)
|
||||
|
||||
raw_button_msg = packer.make_can_msg("BCM_PO_11", 0, {"RAY_LKAS_BTN": 1})
|
||||
assert raw_button_msg[1][0] == 0x10
|
||||
|
||||
def test_non_ray_does_not_use_ray_lkas_signal(self):
|
||||
CP = CarInterface.get_params(CAR.KIA_FORTE_2021_NON_SCC, gen_empty_fingerprint(), [], False, False, False, None)
|
||||
car_state = CarState(CP, CarInterface.get_starpilot_params(CAR.KIA_FORTE_2021_NON_SCC,
|
||||
gen_empty_fingerprint(), [], CP, get_test_toggles()))
|
||||
parser_cycle = SimpleNamespace(
|
||||
vl={
|
||||
"CLU13": {"CF_Clu_LdwsLkasSW": 0},
|
||||
"BCM_PO_11": {"LDA_BTN": 0, "RAY_LKAS_BTN": 1},
|
||||
},
|
||||
ts_nanos={
|
||||
"CLU13": {"CF_Clu_LdwsLkasSW": 1},
|
||||
"BCM_PO_11": {"LDA_BTN": 1, "RAY_LKAS_BTN": 1},
|
||||
},
|
||||
)
|
||||
|
||||
assert not car_state.create_lkas_button_events(parser_cycle, 0)
|
||||
|
||||
def test_hyundai_redneck_cruise_availability(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, *args, **kwargs):
|
||||
@@ -1368,7 +1281,7 @@ class TestHyundaiFingerprint:
|
||||
|
||||
assert CP.startAccel == pytest.approx(1.4)
|
||||
assert CP.vEgoStarting == pytest.approx(0.5)
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.5)
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.35)
|
||||
assert CP.vEgoStopping == pytest.approx(0.3)
|
||||
assert CP.stoppingDecelRate == pytest.approx(0.4)
|
||||
assert kia_ev6_gt_line_longitudinal_tuning(CP.carFingerprint, CP.carVin)
|
||||
@@ -1397,7 +1310,7 @@ class TestHyundaiFingerprint:
|
||||
|
||||
assert CP.startAccel == pytest.approx(1.4)
|
||||
assert CP.vEgoStarting == pytest.approx(0.5)
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.5)
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.35)
|
||||
|
||||
assert kia_ev6_gt_line_longitudinal_tuning(CP.carFingerprint, CP.carVin, testing_ground_active=True)
|
||||
assert not kia_ev6_gt_line_longitudinal_tuning(CAR.KIA_EV6_2025, CP.carVin, testing_ground_active=True)
|
||||
@@ -2537,45 +2450,6 @@ class TestHyundaiFingerprint:
|
||||
assert lfa_parser.can_valid
|
||||
assert lfa_parser.vl["LFA"]["DAMP_FACTOR"] == 100
|
||||
|
||||
controller.long_active_ecu = True
|
||||
cc.longActive = False
|
||||
inactive_msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2)
|
||||
steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in inactive_msgs
|
||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||
assert steering_names == [("LKAS", can_bus.ACAN)]
|
||||
|
||||
controller.frame = 1
|
||||
cc.longActive = True
|
||||
active_msgs = controller.create_canfd_msgs(0, True, 0.44, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2)
|
||||
steering_names = [(controller.packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in active_msgs
|
||||
if controller.packer.dbc.addr_to_msg[addr].name in ("LFA", "LKAS")]
|
||||
assert steering_names == [("LFA", can_bus.ECAN), ("LKAS", can_bus.ACAN)]
|
||||
|
||||
def test_ioniq_6_keeps_lfa_status_when_longitudinal_is_inactive(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.HYUNDAI_IONIQ_6
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 1
|
||||
controller.long_active_ecu = False
|
||||
cc = SimpleNamespace(
|
||||
enabled=False, latActive=False, longActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace(),
|
||||
)
|
||||
cs = SimpleNamespace(
|
||||
stock_lfa_msg=None, stock_lkas_msg=None,
|
||||
out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.park),
|
||||
)
|
||||
|
||||
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False,
|
||||
cc.hudControl, cs, cc, get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
assert any(addr == 0x12A for addr, _, _ in msgs)
|
||||
|
||||
def test_gv70_electrified_longitudinal_uses_hda2_scc_contract(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN
|
||||
@@ -2707,70 +2581,6 @@ class TestHyundaiFingerprint:
|
||||
get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
assert len([msg for msg in msgs if msg[0] == 0x110]) == expected_lkas_msgs
|
||||
|
||||
@pytest.mark.parametrize("standstill", [False, True])
|
||||
def test_sportage_angle_lkas_alt_keeps_inactive_status_in_drive(self, standstill):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
|
||||
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.HYBRID | HyundaiFlags.CANFD_ANGLE_STEERING |
|
||||
HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
|
||||
CP.openpilotLongitudinalControl = False
|
||||
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
can_bus = CanBus(CP)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN)
|
||||
stock_lkas = {
|
||||
"CHECKSUM": 1234,
|
||||
"COUNTER": 42,
|
||||
"LKA_OptUsmSta": 2,
|
||||
"LKA_MODE": 2,
|
||||
"LKA_RcgSta": 3,
|
||||
"LKA_AVAILABLE": 3,
|
||||
"LKA_LHLnWrnSta": 3,
|
||||
"LKA_RHLnWrnSta": 3,
|
||||
"LKA_WARNING": 1,
|
||||
"LKA_HndsoffSnd": 1,
|
||||
"LKA_StrSnd": 1,
|
||||
"LKA_SysIndReq": 4,
|
||||
"LKA_ICON": 2,
|
||||
"FCA_SYSWARN": 1,
|
||||
"StrTqReqVal": 17,
|
||||
"TORQUE_REQUEST": 17,
|
||||
"ActToiSta": 3,
|
||||
"STEER_REQ": 1,
|
||||
"ToiFltSta": 3,
|
||||
"LFA_BUTTON": 1,
|
||||
"LKA_SysWrn": 15,
|
||||
"LKA_ASSIST": 1,
|
||||
"Damping_Gain": 0,
|
||||
"STEER_MODE": 5,
|
||||
"NEW_SIGNAL_2": 0,
|
||||
"LKAS_ANGLE_ACTIVE": 2,
|
||||
"LKA_UsmMod": 3,
|
||||
"HAS_LANE_SAFETY": 1,
|
||||
"ADAS_StrAnglReqVal": 12.3,
|
||||
"ADAS_ACIAnglTqRedcGainVal": 0.42,
|
||||
"DAMP_FACTOR": 0,
|
||||
}
|
||||
cc = SimpleNamespace(enabled=False, latActive=False,
|
||||
actuators=SimpleNamespace(longControlState=LongCtrlState.off),
|
||||
leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace())
|
||||
cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas,
|
||||
out=SimpleNamespace(standstill=standstill, steeringAngleDeg=0.0,
|
||||
gearShifter=structs.CarState.GearShifter.drive))
|
||||
|
||||
msgs = controller.create_canfd_msgs(0, False, 0.0, 0.0, 0.0, 0.0, False, cc.hudControl, cs, cc,
|
||||
get_test_toggles(), lka_icon=1, lfa_icon=1)
|
||||
lkas_msgs = [msg for msg in msgs if msg[0] == 0x110]
|
||||
assert len(lkas_msgs) == 1
|
||||
|
||||
parser.update([(1, lkas_msgs)])
|
||||
assert parser.can_valid
|
||||
assert parser.vl["LKAS_ALT"]["LKA_StrSnd"] == 2
|
||||
assert parser.vl["LKAS_ALT"]["LKA_SysIndReq"] == 1
|
||||
assert parser.vl["LKAS_ALT"]["LKA_RcgSta"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKA_AVAILABLE"] == 0
|
||||
assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1
|
||||
|
||||
def test_ev9_inactive_angle_steering_does_not_suppress_stock_lfa(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.KIA_EV9
|
||||
|
||||
@@ -2,8 +2,6 @@ import re
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag
|
||||
|
||||
# Provenance: portions of HKG angle limits, flags, and platform data are adapted from
|
||||
# sunnypilot/opendbc's hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md.
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
@@ -1000,10 +998,6 @@ KIA_EV6_GT_LINE_LONG_TUNING_VDS_PREFIXES = frozenset({
|
||||
})
|
||||
KIA_EV6_GT_LINE_LONG_TUNING_TESTING_GROUND_ID = "5"
|
||||
|
||||
KIA_RAY_EV_VIN_VDS_PREFIXES = frozenset({
|
||||
"CG81A",
|
||||
})
|
||||
|
||||
|
||||
ALT_BUS_LDA_BUTTON_CARS = frozenset()
|
||||
ALT_BUS_LDA_BUTTON_SWL_STAT_CARS = frozenset()
|
||||
@@ -1018,10 +1012,6 @@ def kia_ev6_gt_line_longitudinal_tuning(car_fingerprint, vin: str, testing_groun
|
||||
return car_fingerprint == CAR.KIA_EV6 and (vin_match or testing_ground_active)
|
||||
|
||||
|
||||
def kia_ray_ev_vin(vin: str) -> bool:
|
||||
return isinstance(vin, str) and len(vin) == 17 and vin[3:8] in KIA_RAY_EV_VIN_VDS_PREFIXES
|
||||
|
||||
|
||||
def get_platform_codes(fw_versions: list[bytes]) -> set[tuple[bytes, bytes | None]]:
|
||||
# Returns unique, platform-specific identification codes for a set of versions
|
||||
codes = set() # (code-Optional[part], date)
|
||||
|
||||
@@ -22,7 +22,7 @@ from opendbc.car.honda.values import CAR as HONDA, HONDA_BOSCH, HondaFlags, Hond
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI, CANFD_CAR, HyundaiFlags, HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, ALT_BUS_LDA_BUTTON_CARS
|
||||
from opendbc.car.mock.values import CAR as MOCK
|
||||
from opendbc.car.subaru.values import CAR as SUBARU, SUBARU_REDNECK_CRUISE_CARS, SubaruSafetyFlags
|
||||
from opendbc.car.subaru.values import CAR as SUBARU, SubaruSafetyFlags
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA, NO_DSU_CAR, TSS2_CAR, UNSUPPORTED_DSU_CAR, ToyotaStarPilotFlags, ToyotaSafetyFlags
|
||||
from opendbc.car.values import PLATFORMS
|
||||
from opendbc.can import CANParser
|
||||
@@ -300,14 +300,6 @@ class CarInterfaceBase(ABC):
|
||||
if getattr(starpilot_toggles, "subaru_sng", False):
|
||||
fp_ret.safetyConfigs[-1].safetyParam |= SubaruSafetyFlags.STOP_AND_GO.value
|
||||
|
||||
fp_ret.redneckCruiseAvailable = candidate in SUBARU_REDNECK_CRUISE_CARS
|
||||
if fp_ret.redneckCruiseAvailable and params.get_bool("SubaruRedneckCruise") and \
|
||||
not CP.openpilotLongitudinalControl:
|
||||
fp_ret.pcmCruiseSpeed = False
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP.safetyConfigs[-1].safetyParam |= SubaruSafetyFlags.REDNECK_CRUISE.value
|
||||
fp_ret.safetyConfigs[-1].safetyParam |= SubaruSafetyFlags.REDNECK_CRUISE.value
|
||||
|
||||
return fp_ret
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -4,7 +4,7 @@ from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, apply_steer_angle_limits_vm, common_fault_avoidance
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, SUBARU_STOP_START_CARS, CanBus, CarControllerParams, SubaruFlags
|
||||
from opendbc.car.subaru.values import CAR, DBC, GLOBAL_ES_ADDR, SUBARU_AVH_CARS, SUBARU_STOP_START_CARS, CanBus, CarControllerParams, SubaruFlags
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
|
||||
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
|
||||
@@ -37,8 +37,9 @@ _STOP_START_STARTUP_DELAY_FRAMES = 100
|
||||
_STOP_START_STARTUP_DEADLINE_FRAMES = 1000
|
||||
_STOP_START_PULSE_FRAMES = 30
|
||||
_STOP_START_PULSE_PERIOD_FRAMES = 5
|
||||
_REDNECK_BUTTON_INTERVAL_FRAMES = 10
|
||||
_REDNECK_BUTTON_COPIES = 2
|
||||
_AVH_STARTUP_DELAY_FRAMES = _STOP_START_STARTUP_DELAY_FRAMES
|
||||
_AVH_STARTUP_DEADLINE_FRAMES = _STOP_START_STARTUP_DEADLINE_FRAMES
|
||||
_AVH_PULSE_MESSAGES = 15 # Match the native 10 Hz AVH frame for roughly 1.5 seconds
|
||||
|
||||
|
||||
def get_safety_CP():
|
||||
@@ -89,7 +90,10 @@ class CarController(CarControllerBase):
|
||||
self.stop_start_initial_state = None
|
||||
self.stop_start_counter = 0
|
||||
self.stop_start_acknowledged = False
|
||||
self.last_redneck_button_frame = 0
|
||||
self.avh_attempted = False
|
||||
self.avh_request_started = False
|
||||
self.avh_last_counter = None
|
||||
self.avh_messages_sent = 0
|
||||
|
||||
def _stop_start_off_request(self, CC, CS, starpilot_toggles):
|
||||
"""Send one bounded Subaru Stop/Start OFF request after ignition.
|
||||
@@ -146,6 +150,55 @@ class CarController(CarControllerBase):
|
||||
self.stop_start_counter = (self.stop_start_counter + 1) % 0x10
|
||||
return msg
|
||||
|
||||
def _avh_on_request(self, CC, CS, starpilot_toggles):
|
||||
"""Send a bounded Subaru AVH ON pulse after ignition.
|
||||
|
||||
The AVH button frame was identified on the 2025 Legacy only. Keep this
|
||||
independent from Stop/Start so the existing Outback request is unchanged.
|
||||
"""
|
||||
if self.CP.carFingerprint not in SUBARU_AVH_CARS or \
|
||||
not getattr(starpilot_toggles, "subaru_avh_on", False) or self.avh_attempted:
|
||||
return None
|
||||
|
||||
if self.frame > _AVH_STARTUP_DEADLINE_FRAMES or getattr(CC, "enabled", False):
|
||||
self.avh_attempted = True
|
||||
return None
|
||||
|
||||
if self.frame < _AVH_STARTUP_DELAY_FRAMES or not getattr(getattr(CS, "out", None), "canValid", True):
|
||||
return None
|
||||
|
||||
out = CS.out
|
||||
if not getattr(out, "standstill", False) or out.gearShifter not in (
|
||||
structs.CarState.GearShifter.park,
|
||||
structs.CarState.GearShifter.neutral,
|
||||
):
|
||||
return None
|
||||
|
||||
avh_msg = getattr(CS, "avh_msg", None)
|
||||
avh_dat = getattr(CS, "avh_dat", None)
|
||||
if not avh_msg or not avh_dat:
|
||||
return None
|
||||
|
||||
if not self.avh_request_started:
|
||||
self.avh_request_started = True
|
||||
self.avh_last_counter = int(avh_msg.get("COUNTER", 0)) % 0x10
|
||||
|
||||
if self.avh_messages_sent >= _AVH_PULSE_MESSAGES:
|
||||
self.avh_attempted = True
|
||||
return None
|
||||
|
||||
counter = int(avh_msg.get("COUNTER", 0)) % 0x10
|
||||
if counter == self.avh_last_counter:
|
||||
return None
|
||||
|
||||
msg = subarucan.create_avh_control(
|
||||
self.packer, avh_msg, raw_dat=avh_dat,
|
||||
counter=counter, bus=CanBus.alt_for_cp(self.CP),
|
||||
)
|
||||
self.avh_last_counter = counter
|
||||
self.avh_messages_sent += 1
|
||||
return msg
|
||||
|
||||
def _reset_legacy_2025_handoff(self):
|
||||
self.driver_override = False
|
||||
self.angle_override_confirm_frames = 0
|
||||
@@ -412,10 +465,6 @@ class CarController(CarControllerBase):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
subaru_redneck_cruise = bool(
|
||||
self.CP.carFingerprint == CAR.SUBARU_IMPREZA_2020 and
|
||||
getattr(starpilot_toggles, "subaru_redneck_cruise", False)
|
||||
)
|
||||
|
||||
can_sends = []
|
||||
|
||||
@@ -423,6 +472,10 @@ class CarController(CarControllerBase):
|
||||
if stop_start_msg is not None:
|
||||
can_sends.append(stop_start_msg)
|
||||
|
||||
avh_msg = self._avh_on_request(CC, CS, starpilot_toggles)
|
||||
if avh_msg is not None:
|
||||
can_sends.append(avh_msg)
|
||||
|
||||
# *** steering ***
|
||||
if (self.frame % self.p.STEER_STEP) == 0:
|
||||
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
|
||||
@@ -480,8 +533,7 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled,
|
||||
self.CP.openpilotLongitudinalControl and not subaru_redneck_cruise,
|
||||
CC.longActive, hud_control.leadVisible,
|
||||
self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible,
|
||||
self.status_bus))
|
||||
|
||||
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.latActive, hud_control.visualAlert,
|
||||
@@ -499,7 +551,7 @@ class CarController(CarControllerBase):
|
||||
can_sends.append(subarucan.create_brake_pedal(self.packer, self.frame // 2, CS.brake_pedal_msg,
|
||||
speed_cmd, pcm_cancel_cmd))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl and not subaru_redneck_cruise:
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 5 == 0:
|
||||
can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg,
|
||||
self.CP.openpilotLongitudinalControl, CC.longActive, cruise_rpm))
|
||||
@@ -515,20 +567,6 @@ class CarController(CarControllerBase):
|
||||
bus = CanBus.alt_for_cp(self.CP) if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else self.main_bus
|
||||
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
|
||||
|
||||
if subaru_redneck_cruise:
|
||||
redneck_button = {
|
||||
1: subarucan.CRUISE_BUTTON_RESUME,
|
||||
2: subarucan.CRUISE_BUTTON_SET,
|
||||
}.get(getattr(CS, "redneck_send_button", 0))
|
||||
cruise_buttons_msg = getattr(CS, "cruise_buttons_msg", None)
|
||||
if redneck_button and cruise_buttons_msg and self.frame - self.last_redneck_button_frame >= _REDNECK_BUTTON_INTERVAL_FRAMES:
|
||||
counter = (int(cruise_buttons_msg["COUNTER"]) + 1) % 0x10
|
||||
for copy_idx in range(_REDNECK_BUTTON_COPIES):
|
||||
can_sends.append(subarucan.create_cruise_buttons(
|
||||
self.packer, counter + copy_idx, cruise_buttons_msg, redneck_button, self.main_bus,
|
||||
))
|
||||
self.last_redneck_button_frame = self.frame
|
||||
|
||||
if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT:
|
||||
# Tester present (keeps eyesight disabled)
|
||||
if self.frame % 100 == 0:
|
||||
|
||||
@@ -1,20 +1,12 @@
|
||||
import copy
|
||||
from cereal import custom
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.subaru.values import DBC, CanBus, SUBARU_REDNECK_CRUISE_CARS, SUBARU_STOP_START_CARS, SubaruFlags
|
||||
from opendbc.car.subaru.values import DBC, CanBus, SUBARU_AVH_CARS, SUBARU_STOP_START_CARS, SubaruFlags
|
||||
from opendbc.car import CanSignalRateCalculator
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
SUBARU_CRUISE_BUTTONS = {
|
||||
"Main": ButtonType.mainCruise,
|
||||
"Set": ButtonType.decelCruise,
|
||||
"Resume": ButtonType.accelCruise,
|
||||
}
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, FPCP):
|
||||
@@ -26,8 +18,8 @@ class CarState(CarStateBase):
|
||||
self.dashlights_msg = {}
|
||||
self.dashlights_dat = b""
|
||||
self.stop_start_state = 0
|
||||
self.cruise_buttons_msg = {}
|
||||
self.cruise_buttons = {button: 0 for button in SUBARU_CRUISE_BUTTONS}
|
||||
self.avh_msg = {}
|
||||
self.avh_dat = b""
|
||||
|
||||
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
|
||||
cp = can_parsers[Bus.pt]
|
||||
@@ -43,6 +35,11 @@ class CarState(CarStateBase):
|
||||
self.dashlights_dat = stop_start_cp.vl_raw["Dashlights"]
|
||||
self.stop_start_state = stop_start_cp.vl["Engine_Stop_Start"]["STOP_START_STATE"]
|
||||
|
||||
if self.CP.carFingerprint in SUBARU_AVH_CARS:
|
||||
avh_cp = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
|
||||
self.avh_msg = copy.copy(avh_cp.vl["AVH"])
|
||||
self.avh_dat = avh_cp.vl_raw["AVH"]
|
||||
|
||||
throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"]
|
||||
ret.gasPressed = throttle_msg["Throttle_Pedal"] > 1e-5
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
@@ -146,17 +143,6 @@ class CarState(CarStateBase):
|
||||
self.es_status_msg = copy.copy(cp_es_brake.vl["ES_Status"])
|
||||
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
|
||||
|
||||
if self.CP.carFingerprint in SUBARU_REDNECK_CRUISE_CARS:
|
||||
cruise_buttons = cp.vl["Cruise_Buttons"]
|
||||
if getattr(starpilot_toggles, "subaru_redneck_cruise", False):
|
||||
ret.buttonEvents = []
|
||||
for button, button_type in SUBARU_CRUISE_BUTTONS.items():
|
||||
ret.buttonEvents.extend(create_button_events(
|
||||
int(bool(cruise_buttons[button])), self.cruise_buttons[button], {1: button_type},
|
||||
))
|
||||
self.cruise_buttons = {button: int(bool(cruise_buttons[button])) for button in SUBARU_CRUISE_BUTTONS}
|
||||
self.cruise_buttons_msg = copy.copy(cruise_buttons)
|
||||
|
||||
if not (self.CP.flags & SubaruFlags.HYBRID):
|
||||
self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"])
|
||||
|
||||
@@ -177,10 +163,11 @@ class CarState(CarStateBase):
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP):
|
||||
avh_messages = [("AVH", 0)] if CP.carFingerprint in SUBARU_AVH_CARS else []
|
||||
parsers = {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main_for_cp(CP)),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt_for_cp(CP))
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], avh_messages, CanBus.alt_for_cp(CP))
|
||||
}
|
||||
if CP.flags & SubaruFlags.D_PLATFORM:
|
||||
parsers[Bus.main] = CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main)
|
||||
|
||||
@@ -3,7 +3,7 @@ from opendbc.car.disable_ecu import disable_ecu
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
from opendbc.car.subaru.carstate import CarState
|
||||
from opendbc.car.subaru.values import CAR, CanBus, GLOBAL_ES_ADDR, SUBARU_STOP_START_CARS, SubaruFlags, SubaruSafetyFlags
|
||||
from opendbc.car.subaru.values import CAR, CanBus, GLOBAL_ES_ADDR, SUBARU_AVH_CARS, SUBARU_STOP_START_CARS, SubaruFlags, SubaruSafetyFlags
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@@ -42,6 +42,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
|
||||
if candidate in SUBARU_STOP_START_CARS:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.STOP_START_BUTTON.value
|
||||
if candidate in SUBARU_AVH_CARS:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.AVH_BUTTON.value
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
|
||||
|
||||
|
||||
@@ -3,10 +3,6 @@ from opendbc.car.subaru.values import CanBus
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
|
||||
CRUISE_BUTTON_MAIN = 1
|
||||
CRUISE_BUTTON_SET = 2
|
||||
CRUISE_BUTTON_RESUME = 3
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_torque, steer_req):
|
||||
values = {
|
||||
@@ -71,19 +67,6 @@ def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long
|
||||
return packer.make_can_msg("ES_Distance", bus, values)
|
||||
|
||||
|
||||
def create_cruise_buttons(packer, frame, cruise_buttons_msg, button, bus=CanBus.main):
|
||||
values = {s: cruise_buttons_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"Signal1",
|
||||
"Signal2",
|
||||
]}
|
||||
values["COUNTER"] = frame % 0x10
|
||||
values["Main"] = button == CRUISE_BUTTON_MAIN
|
||||
values["Set"] = button == CRUISE_BUTTON_SET
|
||||
values["Resume"] = button == CRUISE_BUTTON_RESUME
|
||||
return packer.make_can_msg("Cruise_Buttons", bus, values)
|
||||
|
||||
|
||||
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart,
|
||||
bus=CanBus.main):
|
||||
values = {s: es_lkas_state_msg[s] for s in [
|
||||
@@ -225,6 +208,31 @@ def create_stop_start_control(packer, dashlights_msg, raw_dat=None, counter=None
|
||||
return packer.make_can_msg("Dashlights", bus, values)
|
||||
|
||||
|
||||
def create_avh_control(packer, avh_msg, raw_dat=None, counter=None, bus=CanBus.alt):
|
||||
"""Create the supported Subaru Legacy AVH ON request.
|
||||
|
||||
AVH is carried in the live 0x32b frame. Preserve the other bytes and update
|
||||
only the rolling counter, AVH bit, and Subaru additive checksum.
|
||||
"""
|
||||
if raw_dat:
|
||||
dat = bytearray(raw_dat)
|
||||
if len(dat) != 8:
|
||||
raise ValueError(f"AVH frame must be 8 bytes, got {len(dat)}")
|
||||
if counter is None:
|
||||
counter = (int(avh_msg.get("COUNTER", 0)) + 1) % 0x10
|
||||
dat[1] = (dat[1] & 0xF0) | (counter % 0x10)
|
||||
dat[5] |= 0x20 # AVH, big-endian bit 45
|
||||
dat[0] = ((0x32B & 0xFF) + ((0x32B >> 8) & 0xFF) + sum(dat[1:])) & 0xFF
|
||||
return 0x32B, bytes(dat), bus
|
||||
|
||||
values = dict(avh_msg)
|
||||
if counter is None:
|
||||
counter = (int(values.get("COUNTER", 0)) + 1) % 0x10
|
||||
values["COUNTER"] = counter % 0x10
|
||||
values["AVH"] = 1
|
||||
return packer.make_can_msg("AVH", bus, values)
|
||||
|
||||
|
||||
def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value, bus=CanBus.main):
|
||||
values = {s: es_brake_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
|
||||
@@ -5,7 +5,7 @@ from types import SimpleNamespace
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, fw_versions, gen_empty_fingerprint, structs
|
||||
from opendbc.car import Bus, fw_versions, structs
|
||||
from opendbc.car.fw_query_definitions import StdQueries
|
||||
from opendbc.car.subaru import subarucan
|
||||
from opendbc.car.subaru.carcontroller import CarController
|
||||
@@ -67,56 +67,6 @@ def test_preglobal_sng_does_not_send_standstill_keepalive_without_manual_toggle(
|
||||
assert speed_cmd is False
|
||||
|
||||
|
||||
def test_redneck_cruise_buttons_use_resume_for_increase_and_set_for_decrease():
|
||||
dbc = DBC[CAR.SUBARU_IMPREZA_2020][Bus.pt]
|
||||
packer = CANPacker(dbc)
|
||||
parser = CANParser(dbc, [("Cruise_Buttons", 0)], CanBus.main)
|
||||
stock_buttons = defaultdict(int)
|
||||
|
||||
resume_msg = subarucan.create_cruise_buttons(
|
||||
packer, 1, stock_buttons, subarucan.CRUISE_BUTTON_RESUME, CanBus.main,
|
||||
)
|
||||
parser.update([(1, [resume_msg])])
|
||||
assert parser.vl["Cruise_Buttons"]["Resume"] == 1
|
||||
assert parser.vl["Cruise_Buttons"]["Set"] == 0
|
||||
|
||||
set_msg = subarucan.create_cruise_buttons(
|
||||
packer, 2, stock_buttons, subarucan.CRUISE_BUTTON_SET, CanBus.main,
|
||||
)
|
||||
parser.update([(2, [set_msg])])
|
||||
assert parser.vl["Cruise_Buttons"]["Resume"] == 0
|
||||
assert parser.vl["Cruise_Buttons"]["Set"] == 1
|
||||
|
||||
|
||||
def test_redneck_cruise_is_only_available_on_the_experimental_impreza(monkeypatch):
|
||||
class FakeParams:
|
||||
def __init__(self, **_kwargs):
|
||||
pass
|
||||
|
||||
def get_bool(self, key):
|
||||
return key == "SubaruRedneckCruise"
|
||||
|
||||
monkeypatch.setattr("opendbc.car.interfaces.Params", FakeParams)
|
||||
toggles = SimpleNamespace(subaru_sng=False)
|
||||
|
||||
impreza_cp = CarInterface.get_non_essential_params(CAR.SUBARU_IMPREZA_2020)
|
||||
impreza_fpcp = CarInterface.get_starpilot_params(
|
||||
CAR.SUBARU_IMPREZA_2020, gen_empty_fingerprint(), [], impreza_cp, toggles,
|
||||
)
|
||||
assert impreza_fpcp.redneckCruiseAvailable
|
||||
assert not impreza_fpcp.pcmCruiseSpeed
|
||||
assert impreza_cp.openpilotLongitudinalControl
|
||||
assert impreza_cp.safetyConfigs[0].safetyParam & SubaruSafetyFlags.REDNECK_CRUISE
|
||||
|
||||
old_impreza_cp = CarInterface.get_non_essential_params(CAR.SUBARU_IMPREZA)
|
||||
old_impreza_fpcp = CarInterface.get_starpilot_params(
|
||||
CAR.SUBARU_IMPREZA, gen_empty_fingerprint(), [], old_impreza_cp, toggles,
|
||||
)
|
||||
assert not old_impreza_fpcp.redneckCruiseAvailable
|
||||
assert old_impreza_fpcp.pcmCruiseSpeed
|
||||
assert not old_impreza_cp.openpilotLongitudinalControl
|
||||
|
||||
|
||||
class TestSubaruFingerprint:
|
||||
def test_eyesight_queries_do_not_change_diagnostic_state(self, monkeypatch):
|
||||
camera_requests = [request for request in FW_QUERY_CONFIG.requests if CarParams.Ecu.fwdCamera in request.whitelist_ecus]
|
||||
@@ -244,6 +194,7 @@ def test_outback_2023_uses_d_platform_bus_layout():
|
||||
assert CP.flags & SubaruFlags.D_PLATFORM
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.AVH_BUTTON)
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS)
|
||||
assert CanBus.main_for_cp(CP) == CanBus.alt
|
||||
assert CanBus.angle_for_cp(CP) == CanBus.main
|
||||
@@ -274,6 +225,21 @@ def test_stop_start_inputs_are_captured_for_supported_models(platform):
|
||||
assert car_state.stop_start_state == 3
|
||||
|
||||
|
||||
def test_avh_inputs_are_captured_for_legacy_2025():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
car_state = CarState(CP, None)
|
||||
parsers = car_state.get_can_parsers(CP)
|
||||
raw_avh = bytes.fromhex("230f1c4208800000")
|
||||
parsers[Bus.alt].vl["AVH"]["COUNTER"] = 15
|
||||
parsers[Bus.alt].vl["AVH"]["AVH"] = 0
|
||||
parsers[Bus.alt].vl_raw["AVH"] = raw_avh
|
||||
|
||||
car_state.update(parsers, SimpleNamespace(subaru_sng=False))
|
||||
|
||||
assert car_state.avh_msg["COUNTER"] == 15
|
||||
assert car_state.avh_dat == raw_avh
|
||||
|
||||
|
||||
@pytest.mark.parametrize("platform, expected_bus, start_frame", [
|
||||
(CAR.SUBARU_OUTBACK_2023, CanBus.alt, 101),
|
||||
(CAR.SUBARU_LEGACY_2025, CanBus.alt, 401),
|
||||
@@ -326,6 +292,94 @@ def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expect
|
||||
assert controller.stop_start_acknowledged
|
||||
|
||||
|
||||
def test_avh_request_sets_observed_bit_and_pulses_at_native_rate():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, CP)
|
||||
controller.frame = 101
|
||||
|
||||
class TestActuators:
|
||||
steeringAngleDeg = 0.0
|
||||
|
||||
def as_builder(self):
|
||||
return SimpleNamespace(steeringAngleDeg=self.steeringAngleDeg)
|
||||
|
||||
CC = SimpleNamespace(
|
||||
enabled=False,
|
||||
latActive=False,
|
||||
longActive=False,
|
||||
actuators=TestActuators(),
|
||||
hudControl=SimpleNamespace(leadVisible=False),
|
||||
cruiseControl=SimpleNamespace(cancel=False),
|
||||
)
|
||||
CS = SimpleNamespace(
|
||||
canValid=True,
|
||||
avh_msg={"COUNTER": 15, "AVH": 0},
|
||||
avh_dat=bytes.fromhex("230f1c4208800000"),
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
gearShifter=structs.CarState.GearShifter.park,
|
||||
vEgoRaw=0.0,
|
||||
steeringAngleDeg=0.0,
|
||||
),
|
||||
)
|
||||
toggles = SimpleNamespace(subaru_stop_start_off=False, subaru_avh_on=True, subaru_sng=False)
|
||||
|
||||
# Start the request from the current live counter. AVH is a native 10 Hz
|
||||
# frame, so the controller waits for each next live counter before sending
|
||||
# its matching button frame.
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
avh_msgs = [msg for msg in can_sends if msg[0] == 0x32b]
|
||||
assert not avh_msgs
|
||||
|
||||
CS.avh_msg["COUNTER"] = 0
|
||||
CS.avh_dat = bytes.fromhex("14001c4208800000")
|
||||
controller.frame = 103
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
avh_msgs = [msg for msg in can_sends if msg[0] == 0x32b]
|
||||
assert avh_msgs == [(0x32b, bytes.fromhex("34001c4208a00000"), CanBus.alt)]
|
||||
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("AVH", 0)], CanBus.alt)
|
||||
parser.update([(CanBus.alt, avh_msgs)])
|
||||
assert parser.vl["AVH"]["AVH"] == 1
|
||||
assert parser.vl["AVH"]["COUNTER"] == 0
|
||||
|
||||
controller.frame = 104
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
assert not any(msg[0] == 0x32b for msg in can_sends)
|
||||
|
||||
avh_msgs = []
|
||||
for counter in range(1, 15):
|
||||
CS.avh_msg["COUNTER"] = counter
|
||||
raw_dat = bytearray.fromhex("14001c4208800000")
|
||||
raw_dat[1] = counter
|
||||
raw_dat[0] = ((0x32B & 0xFF) + ((0x32B >> 8) & 0xFF) + sum(raw_dat[1:])) & 0xFF
|
||||
CS.avh_dat = bytes(raw_dat)
|
||||
controller.frame = 103 + (counter * 10)
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
sent = [msg for msg in can_sends if msg[0] == 0x32b]
|
||||
assert len(sent) == 1
|
||||
avh_msgs.extend(sent)
|
||||
|
||||
assert len(avh_msgs) == 14
|
||||
assert [msg[1][1] & 0x0F for msg in avh_msgs] == list(range(1, 15))
|
||||
assert all(msg[1][5] & 0x20 for msg in avh_msgs)
|
||||
assert not controller.avh_attempted
|
||||
|
||||
CS.avh_msg["COUNTER"] = 15
|
||||
CS.avh_dat = bytes.fromhex("230f1c4208800000")
|
||||
controller.frame = 253
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
assert not any(msg[0] == 0x32b for msg in can_sends)
|
||||
assert controller.avh_attempted
|
||||
|
||||
CS.avh_msg["COUNTER"] = 0
|
||||
CS.avh_dat = bytes.fromhex("14001c4208800000")
|
||||
controller.frame = 131
|
||||
_, can_sends = controller.update(CC, CS, 0, toggles)
|
||||
assert not any(msg[0] == 0x32b for msg in can_sends)
|
||||
assert controller.avh_attempted
|
||||
|
||||
|
||||
def test_legacy_2025_uses_gen2_angle_bus_layout():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
parsers = CarState.get_can_parsers(CP)
|
||||
@@ -337,6 +391,7 @@ def test_legacy_2025_uses_gen2_angle_bus_layout():
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA)
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.STOP_START_BUTTON
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.AVH_BUTTON
|
||||
assert CanBus.main_for_cp(CP) == CanBus.main
|
||||
assert CanBus.angle_for_cp(CP) == CanBus.main
|
||||
assert parsers[Bus.pt].bus == CanBus.main
|
||||
|
||||
@@ -89,7 +89,7 @@ class SubaruSafetyFlags(IntFlag):
|
||||
D_PLATFORM_CAMERA = 64
|
||||
FIXED_ANGLE_LIMITS = 128
|
||||
STOP_START_BUTTON = 256
|
||||
REDNECK_CRUISE = 512
|
||||
AVH_BUTTON = 512
|
||||
LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
|
||||
|
||||
|
||||
@@ -276,10 +276,11 @@ SUBARU_STOP_START_CARS = (
|
||||
CAR.SUBARU_LEGACY_2025,
|
||||
)
|
||||
|
||||
SUBARU_REDNECK_CRUISE_CARS = (
|
||||
CAR.SUBARU_IMPREZA_2020,
|
||||
SUBARU_AVH_CARS = (
|
||||
CAR.SUBARU_LEGACY_2025,
|
||||
)
|
||||
|
||||
|
||||
SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
|
||||
SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
|
||||
@@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \
|
||||
CarControllerParams, ToyotaFlags, \
|
||||
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS
|
||||
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR
|
||||
from opendbc.can import CANPacker
|
||||
|
||||
Ecu = structs.CarParams.Ecu
|
||||
@@ -37,9 +37,6 @@ TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -0.06 # m/s^2
|
||||
TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -0.30 # m/s^2
|
||||
TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL = 2.0 # m/s^2
|
||||
TOYOTA_NO_LEAD_CRUISE_SIGN_FLIP_MIN_SET_SPEED_ERROR = 0.35 # m/s
|
||||
TOYOTA_RAV4_LAUNCH_PEDAL_BLEND_SPEED = 5.0 # m/s
|
||||
TOYOTA_RAV4_LAUNCH_PEDAL_SCALE = 0.11
|
||||
TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE = 0.23
|
||||
|
||||
# LKA limits
|
||||
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
|
||||
@@ -75,22 +72,6 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool:
|
||||
) or highlander_sdsu)
|
||||
|
||||
|
||||
def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool:
|
||||
return (
|
||||
auto_hold_enabled and
|
||||
CP.carFingerprint in TOYOTA_AUTO_HOLD_CARS and
|
||||
bool(CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value)
|
||||
)
|
||||
|
||||
|
||||
def get_rav4_interceptor_pedal_scale(v_ego: float) -> float:
|
||||
return float(np.interp(
|
||||
max(float(v_ego), 0.0),
|
||||
[0.0, TOYOTA_RAV4_LAUNCH_PEDAL_BLEND_SPEED, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION],
|
||||
[TOYOTA_RAV4_LAUNCH_PEDAL_SCALE, TOYOTA_RAV4_LOW_SPEED_PEDAL_SCALE, 0.3, 0.0],
|
||||
))
|
||||
|
||||
|
||||
def get_long_tune(CP, params):
|
||||
kiBP = [2., 5.]
|
||||
kiV = [0.5, 0.25]
|
||||
@@ -262,8 +243,11 @@ class CarController(CarControllerBase):
|
||||
self.secoc_prev_reset_counter = 0
|
||||
|
||||
self.doors_locked = False
|
||||
self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value)
|
||||
self.brake_hold_active = False
|
||||
self._brake_hold_counter = 0
|
||||
self._brake_hold_reset = False
|
||||
self._prev_brake_pressed = False
|
||||
|
||||
def _compute_interceptor_gas_cmd(self, CC, CS):
|
||||
if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive):
|
||||
@@ -274,7 +258,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
max_interceptor_gas = 0.5
|
||||
if self.CP.carFingerprint == CAR.TOYOTA_RAV4:
|
||||
pedal_scale = get_rav4_interceptor_pedal_scale(CS.out.vEgo)
|
||||
pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]))
|
||||
elif self.CP.carFingerprint in (CAR.TOYOTA_COROLLA, CAR.TOYOTA_MATRIX_RETROFIT):
|
||||
pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]))
|
||||
else:
|
||||
@@ -315,12 +299,15 @@ class CarController(CarControllerBase):
|
||||
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
|
||||
CS.out.gearShifter not in (PARK, REVERSE))
|
||||
|
||||
if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed:
|
||||
if brake_hold_allowed:
|
||||
self._brake_hold_counter += 1
|
||||
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer
|
||||
elif not brake_hold_allowed:
|
||||
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset
|
||||
self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset
|
||||
else:
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
self._brake_hold_reset = False
|
||||
self._prev_brake_pressed = CS.out.brakePressed
|
||||
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
|
||||
@@ -422,11 +409,8 @@ class CarController(CarControllerBase):
|
||||
# *** gas and brake ***
|
||||
|
||||
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
|
||||
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
|
||||
if self.auto_brake_hold:
|
||||
can_sends.extend(self.create_auto_brake_hold_messages(CS))
|
||||
elif self.brake_hold_active:
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
|
||||
interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS)
|
||||
|
||||
|
||||
@@ -75,7 +75,6 @@ class CarState(CarStateBase):
|
||||
self.distance_button = 0
|
||||
|
||||
self.pcm_follow_distance = 0
|
||||
self.pcm_acc_status = 0
|
||||
|
||||
self.acc_type = 1
|
||||
self.lkas_hud = {}
|
||||
@@ -209,7 +208,6 @@ class CarState(CarStateBase):
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
|
||||
|
||||
prev_pcm_acc_status = self.pcm_acc_status
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
|
||||
if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR):
|
||||
# ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request
|
||||
@@ -266,8 +264,8 @@ class CarState(CarStateBase):
|
||||
buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
|
||||
|
||||
buttonEvents += [
|
||||
*create_button_events(self.pcm_acc_status == 9, prev_pcm_acc_status == 9, {1: ButtonType.accelCruise}),
|
||||
*create_button_events(self.pcm_acc_status == 10, prev_pcm_acc_status == 10, {1: ButtonType.decelCruise}),
|
||||
*create_button_events(self.pcm_acc_status == 9, False, {1: ButtonType.accelCruise}),
|
||||
*create_button_events(self.pcm_acc_status == 10, False, {1: ButtonType.decelCruise}),
|
||||
]
|
||||
|
||||
fp_ret.dashboardSpeedLimit = calculate_speed_limit(cp_cam)
|
||||
@@ -314,9 +312,6 @@ class CarState(CarStateBase):
|
||||
if CP.carFingerprint in DISTANCE_BUTTON_CAR:
|
||||
pt_messages.append(("PCM_CRUISE_4", 1))
|
||||
|
||||
if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value:
|
||||
cam_messages.append(("PRE_COLLISION_2", 50))
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
|
||||
|
||||
@@ -2,9 +2,9 @@ from opendbc.car import Bus, structs, get_safety_config, uds
|
||||
from opendbc.car.toyota.carstate import CarState
|
||||
from opendbc.car.toyota.carcontroller import CarController
|
||||
from opendbc.car.toyota.radar_interface import RadarInterface
|
||||
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
|
||||
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, SECOC_CAR, NO_DSU_CAR, \
|
||||
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
|
||||
ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS
|
||||
ToyotaSafetyFlags, LEGACY_PRIUS_CAR
|
||||
from opendbc.car.disable_ecu import disable_ecu
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
@@ -164,7 +164,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.GAS_INTERCEPTOR.value
|
||||
|
||||
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
||||
if toyota_auto_hold and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
if toyota_auto_hold and candidate in (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR):
|
||||
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
|
||||
|
||||
@@ -10,19 +10,16 @@ from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
|
||||
get_prius_positive_feedforward_scale, \
|
||||
get_rav4_interceptor_pedal_scale, \
|
||||
limit_interceptor_pcm_accel, \
|
||||
limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \
|
||||
limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \
|
||||
update_permit_braking
|
||||
limit_prius_stopping_accel, should_bypass_toyota_long_pid, update_permit_braking
|
||||
from opendbc.car.toyota.carstate import CarState, LKAS_BUTTON_CAR, calculate_interceptor_gas_pressed, create_lkas_button_events
|
||||
from opendbc.car.toyota.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.toyota.interface import CarInterface
|
||||
from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SPEED_SCALE
|
||||
from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
|
||||
from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
|
||||
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
|
||||
ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \
|
||||
get_platform_codes
|
||||
ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, get_platform_codes
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.common.params import Params
|
||||
|
||||
@@ -189,13 +186,12 @@ class TestToyotaInterfaces:
|
||||
if car_model in TSS2_CAR and car_model not in SECOC_CAR:
|
||||
assert dbc[Bus.pt] == "toyota_nodsu_pt_generated"
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate):
|
||||
def test_auto_hold_sets_flag_on_supported_tss2(self):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("ToyotaAutoHold", True)
|
||||
car_params = CarInterface.get_params(
|
||||
candidate,
|
||||
CAR.TOYOTA_CAMRY_TSS2,
|
||||
{bus: {} for bus in range(8)},
|
||||
[],
|
||||
alpha_long=False,
|
||||
@@ -209,28 +205,6 @@ class TestToyotaInterfaces:
|
||||
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
|
||||
can_parsers = CarState.get_can_parsers(car_params)
|
||||
car_state = CarState(car_params, SimpleNamespace(flags=0))
|
||||
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
|
||||
assert "PRE_COLLISION_2" in can_parsers[Bus.cam].vl
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate):
|
||||
params = Params()
|
||||
params.remove("ToyotaAutoHold")
|
||||
car_params = CarInterface.get_params(
|
||||
candidate,
|
||||
{bus: {} for bus in range(8)},
|
||||
[],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
starpilot_toggles=SimpleNamespace(),
|
||||
)
|
||||
|
||||
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
|
||||
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
||||
car_params = CarInterface.get_params(
|
||||
CAR.TOYOTA_PRIUS,
|
||||
@@ -776,84 +750,6 @@ class TestToyotaCarController:
|
||||
|
||||
assert controller.standstill_req is True
|
||||
|
||||
def test_toyota_auto_hold_requires_toggle_supported_car_and_capability(self):
|
||||
CP = SimpleNamespace(
|
||||
carFingerprint=CAR.TOYOTA_CAMRY_TSS2,
|
||||
flags=ToyotaFlags.AUTO_BRAKE_HOLD.value,
|
||||
)
|
||||
|
||||
assert supports_toyota_auto_hold(CP, True)
|
||||
assert supports_toyota_auto_hold(SimpleNamespace(
|
||||
carFingerprint=CAR.TOYOTA_RAV4,
|
||||
flags=ToyotaFlags.AUTO_BRAKE_HOLD.value,
|
||||
), True)
|
||||
assert supports_toyota_auto_hold(SimpleNamespace(
|
||||
carFingerprint=CAR.TOYOTA_RAV4H,
|
||||
flags=ToyotaFlags.AUTO_BRAKE_HOLD.value,
|
||||
), True)
|
||||
assert not supports_toyota_auto_hold(CP, False)
|
||||
assert not supports_toyota_auto_hold(SimpleNamespace(
|
||||
carFingerprint=CAR.TOYOTA_CAMRY_TSS2,
|
||||
flags=0,
|
||||
), True)
|
||||
assert not supports_toyota_auto_hold(SimpleNamespace(
|
||||
carFingerprint=CAR.TOYOTA_CAMRY,
|
||||
flags=ToyotaFlags.AUTO_BRAKE_HOLD.value,
|
||||
), True)
|
||||
|
||||
assert TOYOTA_AUTO_HOLD_CARS >= {CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H}
|
||||
|
||||
def test_toyota_auto_hold_latches_after_brake_press_until_gas(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
cruiseState=SimpleNamespace(available=True, enabled=False),
|
||||
gasPressed=False,
|
||||
brakePressed=True,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
assert controller.brake_hold_active
|
||||
|
||||
cs.out.brakePressed = False
|
||||
controller.frame = 2
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
assert controller.brake_hold_active
|
||||
|
||||
cs.out.gasPressed = True
|
||||
controller.frame = 4
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
assert not controller.brake_hold_active
|
||||
|
||||
def test_toyota_auto_hold_does_not_trigger_without_brake_press(self):
|
||||
controller = self._make_controller()
|
||||
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
cruiseState=SimpleNamespace(available=True, enabled=False),
|
||||
gasPressed=False,
|
||||
brakePressed=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
)
|
||||
|
||||
controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
|
||||
assert not controller.brake_hold_active
|
||||
|
||||
def test_prius_resume_request_releases_standstill_latch(self):
|
||||
controller = self._make_controller(standstill_req=True, last_standstill=True)
|
||||
|
||||
@@ -997,12 +893,14 @@ class TestToyotaCarController:
|
||||
controller.frame = 0
|
||||
controller.brake_hold_active = False
|
||||
controller._brake_hold_counter = 0
|
||||
controller._brake_hold_reset = False
|
||||
controller._prev_brake_pressed = False
|
||||
cs = SimpleNamespace(
|
||||
out=SimpleNamespace(
|
||||
standstill=True,
|
||||
cruiseState=SimpleNamespace(available=True, enabled=False),
|
||||
gasPressed=False,
|
||||
brakePressed=True,
|
||||
brakePressed=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
),
|
||||
pre_collision_2={},
|
||||
@@ -1042,27 +940,6 @@ class TestToyotaCarController:
|
||||
|
||||
assert 0.0 < gas_cmd <= 0.5
|
||||
|
||||
def test_rav4_interceptor_launch_mapping_is_softer_than_generic_mapping(self):
|
||||
controller = self._make_controller()
|
||||
controller.CP.enableGasInterceptorDEPRECATED = True
|
||||
controller.CP.carFingerprint = CAR.TOYOTA_RAV4
|
||||
controller.accel = 1.5
|
||||
|
||||
rav4_gas = controller._compute_interceptor_gas_cmd(
|
||||
SimpleNamespace(longActive=True),
|
||||
SimpleNamespace(out=SimpleNamespace(standstill=False, vEgo=1.0)),
|
||||
)
|
||||
|
||||
controller.CP.carFingerprint = CAR.TOYOTA_AVALON_2019
|
||||
generic_gas = controller._compute_interceptor_gas_cmd(
|
||||
SimpleNamespace(longActive=True),
|
||||
SimpleNamespace(out=SimpleNamespace(standstill=False, vEgo=1.0)),
|
||||
)
|
||||
|
||||
assert rav4_gas < generic_gas
|
||||
assert get_rav4_interceptor_pedal_scale(5.0) == pytest.approx(0.23)
|
||||
assert get_rav4_interceptor_pedal_scale(MIN_ACC_SPEED) == pytest.approx(0.3)
|
||||
|
||||
def test_interceptor_corolla_scales_with_accel_request_when_pedal_enables_sng(self):
|
||||
controller = self._make_controller()
|
||||
controller.CP.enableGasInterceptorDEPRECATED = True
|
||||
|
||||
@@ -624,11 +624,6 @@ ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL)
|
||||
|
||||
SECOC_CAR = CAR.with_flags(ToyotaFlags.SECOC)
|
||||
|
||||
TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | {
|
||||
CAR.TOYOTA_RAV4,
|
||||
CAR.TOYOTA_RAV4H,
|
||||
}
|
||||
|
||||
# no resume button press required
|
||||
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
|
||||
|
||||
|
||||
@@ -11,6 +11,7 @@ TransmissionType = structs.CarParams.TransmissionType
|
||||
# Must match VOLVO_SPEED_TO_MS in opendbc/safety/modes/volvo.h.
|
||||
SPEED_TO_MS = 0.003977
|
||||
STEERING_PRESSED_THRESHOLD = 2
|
||||
STEERING_DISENGAGE_THRESHOLD = 5
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
@@ -74,9 +75,11 @@ class CarState(CarStateBase):
|
||||
ret.steeringAngleDeg = cp_party.vl['PSCM']['PSCM_ANGLE_SENSOR'] # openpilot expects a negative value for a right turn
|
||||
#ret.steeringAngleDeg = cp_party.vl['SAS']['SAS_ANGLE_SENSOR']
|
||||
|
||||
# Driver steering torque feedback (used for driver override detection)
|
||||
ret.steeringTorque = -cp_party.vl['DRIVER_INPUT']['STEERING_DRIVER_INPUT'] # Car right turn is negative, openpilot right turn is positive
|
||||
driver_input = abs(cp_party.vl['DRIVER_INPUT']['STEERING_DRIVER_INPUT'])
|
||||
ret.steeringPressed = driver_input > STEERING_PRESSED_THRESHOLD
|
||||
ret.steeringDisengage = driver_input > STEERING_DISENGAGE_THRESHOLD
|
||||
|
||||
# EPS status - placeholder until actual signal is found
|
||||
self.eps_active = True # Assume EPS is active for now
|
||||
|
||||
@@ -1,5 +1,10 @@
|
||||
CM_ "IMPORT _subaru_global.dbc";
|
||||
|
||||
BO_ 811 AVH: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ AVH : 45|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 72 Transmission: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -1497,7 +1497,6 @@ BO_ 913 BCM_PO_11: 8 Vector__XXX
|
||||
SG_ BCM_Door_Dri_Status : 5|1@0+ (1,0) [0|1] "" PT_ESC_ABS
|
||||
SG_ BCM_Shift_R_MT_SW_Status : 39|2@0+ (1,0) [0|3] "" PT_ESC_ABS
|
||||
SG_ LDA_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RAY_LKAS_BTN : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 1426 LABEL11: 8 XXX
|
||||
SG_ CC_React : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
@@ -1497,7 +1497,6 @@ BO_ 913 BCM_PO_11: 8 Vector__XXX
|
||||
SG_ BCM_Door_Dri_Status : 5|1@0+ (1,0) [0|1] "" PT_ESC_ABS
|
||||
SG_ BCM_Shift_R_MT_SW_Status : 39|2@0+ (1,0) [0|3] "" PT_ESC_ABS
|
||||
SG_ LDA_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RAY_LKAS_BTN : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1426 LABEL11: 8 XXX
|
||||
SG_ CC_React : 34|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
@@ -1,25 +0,0 @@
|
||||
VERSION ""
|
||||
|
||||
NS_ :
|
||||
NS_DESC_
|
||||
CM_
|
||||
BA_DEF_
|
||||
BA_
|
||||
VAL_
|
||||
|
||||
BS_:
|
||||
|
||||
BU_: XXX
|
||||
|
||||
BO_ 1157 LFAHDA_MFC: 8 XXX
|
||||
SG_ HDA_USM : 0|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ HDA_Active : 2|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ HDA_Icon_State : 3|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ HDA_Chime : 7|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ HDA_VSetReq : 8|8@1+ (1,0) [0|255] "km/h" XXX
|
||||
SG_ LFA_SysWarning : 16|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ HDA_Icon_Wheel : 20|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ HDA_LdwSysState : 21|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ LFA_Icon_State : 24|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ LFA_USM : 27|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ HDA_SysWarning : 29|2@1+ (1,0) [0|3] "" XXX
|
||||
@@ -307,6 +307,11 @@ VAL_ 544 AEB_Status 12 "AEB related" 8 "AEB actuation" 4 "AEB related" 0 "No AEB
|
||||
|
||||
CM_ "subaru_global_2017.dbc starts here";
|
||||
|
||||
BO_ 811 AVH: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ AVH : 45|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 72 Transmission: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
@@ -2,12 +2,6 @@
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// StarPilot's extended Ford curvature enforcement below is substantially adapted from
|
||||
// BluePilot bp-7.0 panda work, principally Alan Polk's 8f8d6d15f0a590f42b78de964ffb0d0af7f5d63d
|
||||
// See /CREDITS.md and /THIRD_PARTY_NOTICES.md. This comment does not attribute the surrounding
|
||||
// upstream openpilot code.
|
||||
|
||||
|
||||
// Safety-relevant CAN messages for Ford vehicles.
|
||||
#define FORD_EngBrakeData 0x165U // RX from PCM, for driver brake pedal and cruise state
|
||||
#define FORD_EngVehicleSpThrottle 0x204U // RX from PCM, for driver throttle input
|
||||
@@ -94,8 +88,10 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
|
||||
static bool ford_lka_steering = false;
|
||||
static bool ford_extended_lateral = false;
|
||||
static bool ford_angle_mode = false;
|
||||
static bool ford_longitudinal = false;
|
||||
static bool ford_cancel_resume_button = false;
|
||||
static int16_t ford_shadow_curvature = 0;
|
||||
|
||||
// Curvature rate limits
|
||||
#define FORD_LIMITS(limit_lateral_acceleration) { \
|
||||
@@ -142,6 +138,38 @@ static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
|
||||
|
||||
static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false);
|
||||
|
||||
static int ford_desired_path_angle_last = 0;
|
||||
|
||||
static bool ford_path_angle_checks(int desired_path_angle, bool steer_control_enabled) {
|
||||
bool violation = false;
|
||||
if (steer_control_enabled) {
|
||||
float speed = ((float)vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0;
|
||||
const struct lookup_t path_angle_rate = {
|
||||
.x = {10., 15., 25.},
|
||||
.y = {0.0561, 0.04335, 0.00918},
|
||||
};
|
||||
int max_delta = (safety_interpolate(path_angle_rate, speed) * 2000.0) + 1.0;
|
||||
violation |= safety_max_limit_check(desired_path_angle,
|
||||
ford_desired_path_angle_last + max_delta,
|
||||
ford_desired_path_angle_last - max_delta);
|
||||
} else {
|
||||
violation |= desired_path_angle != 0;
|
||||
}
|
||||
ford_desired_path_angle_last = violation ? 0 : desired_path_angle;
|
||||
return violation;
|
||||
}
|
||||
|
||||
static bool ford_shadow_curvature_check(int desired_curvature, bool steer_control_enabled,
|
||||
const AngleSteeringLimits limits) {
|
||||
if (steer_control_enabled && limits.enforce_angle_error &&
|
||||
((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) {
|
||||
int lowest_allowed = angle_meas.min - limits.max_angle_error - 1;
|
||||
int highest_allowed = angle_meas.max + limits.max_angle_error + 1;
|
||||
return safety_max_limit_check(desired_curvature, highest_allowed, lowest_allowed);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static void ford_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == FORD_MAIN_BUS) {
|
||||
// Update in motion state from standstill signal
|
||||
@@ -272,8 +300,10 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
if (!ford_lka_steering) {
|
||||
ford_angle_mode = (msg->data[4] & 0x1U) != 0U;
|
||||
ford_extended_lateral = (msg->data[4] & 0x2U) != 0U;
|
||||
if ((msg->data[4] & 0x1U) != 0U) {
|
||||
ford_shadow_curvature = (int16_t)((msg->data[5] << 8) | msg->data[6]);
|
||||
if (ford_angle_mode && !ford_extended_lateral) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
@@ -297,9 +327,20 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
if (ford_extended_lateral) {
|
||||
violation |= desired_path_offset != 0;
|
||||
violation |= (desired_curvature_rate < -4096) || (desired_curvature_rate > 4095);
|
||||
violation |= desired_path_angle != 0;
|
||||
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
|
||||
FORD_EXTENDED_STEERING_LIMITS);
|
||||
violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled);
|
||||
if (ford_angle_mode) {
|
||||
violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047);
|
||||
violation |= desired_curvature != 0;
|
||||
violation |= steer_control_enabled && !(aol_allowed || controls_allowed);
|
||||
int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05);
|
||||
violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled,
|
||||
FORD_EXTENDED_STEERING_LIMITS);
|
||||
desired_angle_last = 0;
|
||||
} else {
|
||||
violation |= desired_path_angle != 0;
|
||||
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
|
||||
FORD_EXTENDED_STEERING_LIMITS);
|
||||
}
|
||||
if (!steer_control_enabled) {
|
||||
violation |= (desired_curvature != 0) || (desired_curvature_rate != 0);
|
||||
}
|
||||
@@ -336,9 +377,20 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
if (ford_extended_lateral) {
|
||||
violation |= desired_path_offset != 0;
|
||||
violation |= (desired_curvature_rate < -1024) || (desired_curvature_rate > 1023);
|
||||
violation |= desired_path_angle != 0;
|
||||
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
|
||||
FORD_CANFD_EXTENDED_STEERING_LIMITS);
|
||||
violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled);
|
||||
if (ford_angle_mode) {
|
||||
violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047);
|
||||
violation |= desired_curvature != 0;
|
||||
violation |= steer_control_enabled && !(aol_allowed || controls_allowed);
|
||||
int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05);
|
||||
violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled,
|
||||
FORD_CANFD_EXTENDED_STEERING_LIMITS);
|
||||
desired_angle_last = 0;
|
||||
} else {
|
||||
violation |= desired_path_angle != 0;
|
||||
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
|
||||
FORD_CANFD_EXTENDED_STEERING_LIMITS);
|
||||
}
|
||||
if (!steer_control_enabled) {
|
||||
violation |= (desired_curvature != 0) || (desired_curvature_rate != 0);
|
||||
}
|
||||
@@ -409,7 +461,10 @@ static safety_config ford_init(uint16_t param) {
|
||||
const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
|
||||
ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING);
|
||||
ford_extended_lateral = false;
|
||||
ford_angle_mode = false;
|
||||
ford_cancel_resume_button = false;
|
||||
ford_shadow_curvature = 0;
|
||||
ford_desired_path_angle_last = 0;
|
||||
|
||||
ford_longitudinal = false;
|
||||
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
// Provenance: portions of HKG angle-command safety are adapted from sunnypilot/opendbc's
|
||||
// hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
|
||||
@@ -35,7 +35,6 @@
|
||||
#define MSG_SUBARU_ES_DashStatus 0x321U
|
||||
#define MSG_SUBARU_ES_LKAS_State 0x322U
|
||||
#define MSG_SUBARU_ES_Infotainment 0x323U
|
||||
#define MSG_SUBARU_Cruise_Buttons 0x146U
|
||||
|
||||
#define MSG_SUBARU_ES_UDS_Request 0x787U
|
||||
|
||||
@@ -43,6 +42,7 @@
|
||||
#define MSG_SUBARU_ES_STATIC_1 0x22aU
|
||||
#define MSG_SUBARU_ES_STATIC_2 0x325U
|
||||
#define MSG_SUBARU_Dashlights 0x390U
|
||||
#define MSG_SUBARU_AVH 0x32bU
|
||||
|
||||
#define SUBARU_MAIN_BUS 0U
|
||||
#define SUBARU_ALT_BUS 1U
|
||||
@@ -57,9 +57,6 @@
|
||||
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
|
||||
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_REDNECK_TX_MSGS() \
|
||||
{MSG_SUBARU_Cruise_Buttons, SUBARU_MAIN_BUS, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_D_PLATFORM_ANGLE_TX_MSGS(bus) \
|
||||
{MSG_SUBARU_ES_LKAS_ANGLE, bus, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_DashStatus, bus, 8, .check_relay = true}, \
|
||||
@@ -69,6 +66,13 @@
|
||||
#define SUBARU_STOP_START_TX_MSGS(bus) \
|
||||
{MSG_SUBARU_Dashlights, bus, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_AVH_TX_MSGS(bus) \
|
||||
{MSG_SUBARU_AVH, bus, 8, .check_relay = false}, \
|
||||
|
||||
#define SUBARU_STOP_START_AVH_TX_MSGS(bus) \
|
||||
SUBARU_STOP_START_TX_MSGS(bus) \
|
||||
SUBARU_AVH_TX_MSGS(bus)
|
||||
|
||||
#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
|
||||
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = true}, \
|
||||
{MSG_SUBARU_ES_Brake, alt_bus, 8, .check_relay = true}, \
|
||||
@@ -117,7 +121,7 @@ static bool subaru_lkas_angle = false;
|
||||
static bool subaru_d_platform = false;
|
||||
static bool subaru_fixed_angle_limits = false;
|
||||
static bool subaru_stop_start_button = false;
|
||||
static bool subaru_redneck_cruise = false;
|
||||
static bool subaru_avh_button = false;
|
||||
|
||||
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint8_t)msg->data[0];
|
||||
@@ -302,9 +306,10 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_SUBARU_Cruise_Buttons) {
|
||||
violation |= !subaru_redneck_cruise;
|
||||
violation |= msg->bus != SUBARU_MAIN_BUS;
|
||||
if (msg->addr == MSG_SUBARU_AVH) {
|
||||
violation |= !subaru_avh_button;
|
||||
violation |= msg->bus != (subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS);
|
||||
violation |= !GET_BIT(msg, 45U);
|
||||
violation |= subaru_get_checksum(msg) != subaru_compute_checksum(msg);
|
||||
}
|
||||
|
||||
@@ -320,19 +325,6 @@ static safety_config subaru_init(uint16_t param) {
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_REDNECK_TX_MSGS_CONFIG[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_REDNECK_TX_MSGS()
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_REDNECK_STOP_AND_GO_TX_MSGS_CONFIG[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_REDNECK_TX_MSGS()
|
||||
SUBARU_STOP_AND_GO_ADDITIONAL_TX_MSGS()
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_LONG_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
|
||||
SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
@@ -371,6 +363,12 @@ static safety_config subaru_init(uint16_t param) {
|
||||
SUBARU_STOP_START_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_GEN2_LKAS_ANGLE_STOP_START_AVH_TX_MSGS[] = {
|
||||
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
SUBARU_STOP_START_AVH_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS[] = {
|
||||
SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
@@ -382,6 +380,12 @@ static safety_config subaru_init(uint16_t param) {
|
||||
SUBARU_STOP_START_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_D_PLATFORM_ANGLE_STOP_START_AVH_MAIN_TX_MSGS[] = {
|
||||
SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_MAIN_BUS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
SUBARU_STOP_START_AVH_TX_MSGS(SUBARU_ALT_BUS)
|
||||
};
|
||||
|
||||
static const CanMsg SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS[] = {
|
||||
SUBARU_D_PLATFORM_ANGLE_TX_MSGS(SUBARU_CAM_BUS)
|
||||
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
|
||||
@@ -429,8 +433,8 @@ static safety_config subaru_init(uint16_t param) {
|
||||
const uint16_t SUBARU_PARAM_STOP_START_BUTTON = 256;
|
||||
subaru_stop_start_button = GET_FLAG(param, SUBARU_PARAM_STOP_START_BUTTON);
|
||||
|
||||
const uint16_t SUBARU_PARAM_REDNECK_CRUISE = 512;
|
||||
subaru_redneck_cruise = GET_FLAG(param, SUBARU_PARAM_REDNECK_CRUISE);
|
||||
const uint16_t SUBARU_PARAM_AVH_BUTTON = 512;
|
||||
subaru_avh_button = GET_FLAG(param, SUBARU_PARAM_AVH_BUTTON);
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
|
||||
@@ -439,19 +443,19 @@ static safety_config subaru_init(uint16_t param) {
|
||||
|
||||
safety_config ret;
|
||||
if (subaru_lkas_angle) {
|
||||
ret = subaru_d_platform ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS) : \
|
||||
ret = subaru_d_platform ? (subaru_stop_start_button ? (subaru_avh_button ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_AVH_MAIN_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_STOP_START_MAIN_TX_MSGS)) : \
|
||||
(subaru_d_platform_camera ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_CAMERA_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_MAIN_TX_MSGS))) : \
|
||||
subaru_gen2 ? (subaru_stop_start_button ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS) : \
|
||||
subaru_gen2 ? (subaru_stop_start_button ? (subaru_avh_button ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_STOP_START_AVH_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_STOP_START_TX_MSGS)) : \
|
||||
BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS)) : \
|
||||
BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
|
||||
} else if (subaru_gen2) {
|
||||
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
|
||||
} else {
|
||||
ret = subaru_redneck_cruise ? (subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_REDNECK_STOP_AND_GO_TX_MSGS_CONFIG) : \
|
||||
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_REDNECK_TX_MSGS_CONFIG)) : \
|
||||
subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LONG_TX_MSGS) : \
|
||||
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LONG_TX_MSGS) : \
|
||||
subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_STOP_AND_GO_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
|
||||
}
|
||||
|
||||
@@ -224,16 +224,10 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
if (msg->addr == 0x368U) {
|
||||
const int cruise_state = (msg->data[1] >> 4) & 0x0FU;
|
||||
const int cruise_state = (msg->data[1] >> 4) & 0x07U;
|
||||
if (cruise_state == 3) {
|
||||
vehicle_moving = false;
|
||||
}
|
||||
acc_main_on = (cruise_state == 1) ||
|
||||
(cruise_state == 2) ||
|
||||
(cruise_state == 3) ||
|
||||
(cruise_state == 4) ||
|
||||
(cruise_state == 6) ||
|
||||
(cruise_state == 7);
|
||||
}
|
||||
|
||||
if (msg->addr == 0x118U) {
|
||||
|
||||
@@ -43,6 +43,7 @@
|
||||
#define VOLVO_ANGLE_DEG_TO_CAN 17.869907f
|
||||
#define VOLVO_MAX_ANGLE_CAN 9650
|
||||
#define VOLVO_RELAY_ANGLE_TOLERANCE 54 // approximately 3 degrees
|
||||
#define VOLVO_DRIVER_OVERRIDE 5
|
||||
|
||||
|
||||
// CAN bus definitions for Volvo
|
||||
@@ -82,6 +83,8 @@ static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = {
|
||||
};
|
||||
|
||||
static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
// Monitor the vehicle state required for cruise, disengagement, and angle
|
||||
// safety. All steering TX frames are separately constrained in volvo_tx_hook.
|
||||
|
||||
// Main bus (bus 0) messages
|
||||
if (msg->bus == VOLVO_MAIN_BUS) {
|
||||
@@ -145,11 +148,13 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
// DRIVER_INPUT is the signal consumed by carstate.py for driver torque.
|
||||
// The PSCM frame's DRIVER_INPUT_DEVIATION is a different signal and must
|
||||
// not be substituted here: doing so leaves the hardware disengage path blind.
|
||||
if (msg->addr == VOLVO_DRIVER_INPUT) {
|
||||
// STEERING_DRIVER_INPUT is a Motorola signal starting at bit 55. The
|
||||
// DBC also carries a +1 offset, so its raw byte is data[6].
|
||||
const int driver_input = to_signed(msg->data[6], 8) + 1;
|
||||
update_sample(&torque_driver, driver_input);
|
||||
steering_disengage = SAFETY_ABS(driver_input) > VOLVO_DRIVER_OVERRIDE;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -170,11 +170,6 @@ class TestFordSafetyBase(common.CarSafetyTest):
|
||||
}
|
||||
return self.packer.make_can_msg_safety("Lane_Assist_Data1", 0, values)
|
||||
|
||||
def _extended_lka_msg(self, angle_mode=False):
|
||||
msg = self._lkas_command_msg(0)
|
||||
msg[0].data[4] |= 0x2 | int(angle_mode)
|
||||
return msg
|
||||
|
||||
# LCA command
|
||||
def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float):
|
||||
if self.STEER_MESSAGE == MSG_LateralMotionControl:
|
||||
@@ -381,25 +376,6 @@ class TestFordSafetyBase(common.CarSafetyTest):
|
||||
should_tx |= self.LKA_STEERING and controls_allowed and action in (2, 4)
|
||||
self.assertEqual(should_tx, self._tx(self._lkas_command_msg(action)))
|
||||
|
||||
def test_extended_angle_mode_rejected(self):
|
||||
if self.LKA_STEERING:
|
||||
return
|
||||
|
||||
self.assertTrue(self._tx(self._extended_lka_msg()))
|
||||
self.assertFalse(self._tx(self._extended_lka_msg(angle_mode=True)))
|
||||
|
||||
def test_extended_curvature_signals(self):
|
||||
if self.LKA_STEERING:
|
||||
return
|
||||
|
||||
speed = 15.0
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_curvature_measurement(0.0, speed)
|
||||
self.assertTrue(self._tx(self._extended_lka_msg()))
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.001, 0.0005)))
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.1, 0.0, 0.001, 0.0005)))
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.001, 0.0005)))
|
||||
|
||||
def test_acc_buttons(self):
|
||||
for allowed in (0, 1):
|
||||
self.safety.set_controls_allowed(allowed)
|
||||
@@ -467,6 +443,39 @@ class TestFordCANFDStockSafety(TestFordSafetyBase):
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _extended_lka_msg(self, angle_mode=False, shadow_curvature=0.0):
|
||||
msg = self._lkas_command_msg(0)
|
||||
raw_shadow = int(round(shadow_curvature / 1e-6)) & 0xFFFF
|
||||
msg[0].data[4] |= 0x2 | int(angle_mode)
|
||||
msg[0].data[5] = raw_shadow >> 8
|
||||
msg[0].data[6] = raw_shadow & 0xFF
|
||||
return msg
|
||||
|
||||
def test_extended_curvature_signals(self):
|
||||
speed = 15.0
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_curvature_measurement(0.0, speed)
|
||||
self.assertTrue(self._tx(self._extended_lka_msg()))
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.001, 0.0005)))
|
||||
|
||||
self.assertTrue(self._tx(self._extended_lka_msg()))
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.1, 0.0, 0.001, 0.0005)))
|
||||
|
||||
def test_extended_angle_signals(self):
|
||||
speed = 15.0
|
||||
curvature = 0.005
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._reset_curvature_measurement(curvature, speed)
|
||||
self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature)))
|
||||
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.0, 0.0)))
|
||||
|
||||
self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature)))
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.2, 0.0, 0.0)))
|
||||
|
||||
self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=-curvature)))
|
||||
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.01, 0.0, 0.0)))
|
||||
|
||||
|
||||
class TestFordStockSafety(TestFordSafetyBase):
|
||||
STEER_MESSAGE = MSG_LateralMotionControl
|
||||
STOCK_LONGITUDINAL = True
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
# Provenance: portions of HKG angle-safety tests are adapted from sunnypilot/opendbc's
|
||||
# hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md.
|
||||
from parameterized import parameterized_class
|
||||
import unittest
|
||||
import numpy as np
|
||||
|
||||
@@ -37,6 +37,7 @@ class SubaruMsg(enum.IntEnum):
|
||||
ES_STATIC_1 = 0x22a
|
||||
ES_STATIC_2 = 0x325
|
||||
Dashlights = 0x390
|
||||
AVH = 0x32b
|
||||
|
||||
|
||||
SUBARU_MAIN_BUS = 0
|
||||
@@ -385,6 +386,20 @@ class TestSubaruGen2FixedAngleStopStartSafety(TestSubaruGen2FixedAngleSafety):
|
||||
self.assertFalse(self._tx(self._stop_start_msg(False)))
|
||||
|
||||
|
||||
class TestSubaruGen2FixedAngleStopStartAvhSafety(TestSubaruGen2FixedAngleStopStartSafety):
|
||||
FLAGS = TestSubaruGen2FixedAngleStopStartSafety.FLAGS | SubaruSafetyFlags.AVH_BUTTON
|
||||
TX_MSGS = TestSubaruGen2FixedAngleStopStartSafety.TX_MSGS + [[SubaruMsg.AVH, SUBARU_ALT_BUS]]
|
||||
|
||||
def _avh_msg(self, pressed):
|
||||
return self.packer.make_can_msg_safety(
|
||||
"AVH", SUBARU_ALT_BUS, {"COUNTER": 0, "AVH": pressed},
|
||||
)
|
||||
|
||||
def test_avh_tx_requires_pressed_bit(self):
|
||||
self.assertTrue(self._tx(self._avh_msg(True)))
|
||||
self.assertFalse(self._tx(self._avh_msg(False)))
|
||||
|
||||
|
||||
class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM
|
||||
ALT_MAIN_BUS = SUBARU_ALT_BUS
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car.tesla.preap.interface import SAFETY_TESLA_PREAP
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
import opendbc.safety.tests.common as common
|
||||
|
||||
@@ -37,12 +36,6 @@ class TestTeslaPreAPSafety(common.SafetyTestBase):
|
||||
dat[0] = lever & 0x3F
|
||||
return common.make_msg(0, 0x45, 8, dat)
|
||||
|
||||
@staticmethod
|
||||
def _di_state_msg(cruise_state: int):
|
||||
dat = bytearray(8)
|
||||
dat[1] = (cruise_state & 0x0F) << 4
|
||||
return common.make_msg(0, 0x368, 8, dat)
|
||||
|
||||
@staticmethod
|
||||
def _steering_status_msg(hands_on_level: int = 0, eac_status: int = 1, eac_error_code: int = 0, angle_tenths: int = 0):
|
||||
raw_angle = angle_tenths + 8192
|
||||
@@ -130,30 +123,6 @@ class TestTeslaPreAPSafety(common.SafetyTestBase):
|
||||
self.assertTrue(self._tx(self._epas_control_msg(1)))
|
||||
self.assertFalse(self._tx(self._epas_control_msg(2)))
|
||||
|
||||
def test_always_on_lateral(self):
|
||||
ENABLED, OFF = 2, 0
|
||||
self.safety.set_controls_allowed(False)
|
||||
|
||||
self.safety.set_alternative_experience(0)
|
||||
self.assertTrue(self._rx(self._di_state_msg(ENABLED)))
|
||||
self.assertFalse(self._tx(self._steer_cmd_msg(0, 1)))
|
||||
|
||||
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
|
||||
self.assertTrue(self._rx(self._di_state_msg(OFF)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertFalse(self._tx(self._steer_cmd_msg(0, 1)))
|
||||
|
||||
self.assertTrue(self._rx(self._di_state_msg(ENABLED)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self._tx(self._steer_cmd_msg(0, 1)))
|
||||
|
||||
self.assertTrue(self._rx(self._di_state_msg(OFF)))
|
||||
self.assertFalse(self._tx(self._steer_cmd_msg(0, 1)))
|
||||
|
||||
self.assertTrue(self._rx(self._di_state_msg(9)))
|
||||
self.assertFalse(self.safety.get_aol_allowed())
|
||||
self.assertFalse(self._tx(self._steer_cmd_msg(0, 1)))
|
||||
|
||||
def test_aeb_is_blocked(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertTrue(self._tx(self._long_msg(0)))
|
||||
|
||||
@@ -211,17 +211,21 @@ class TestVolvoSafetyBase(common.CarSafetyTest):
|
||||
self.assertTrue(self._tx(valid))
|
||||
self.assertFalse(self._tx(invalid))
|
||||
|
||||
def test_driver_input_is_a_normal_override(self):
|
||||
def test_driver_override_disengages_controls(self):
|
||||
def driver_input_msg(value):
|
||||
return self.mid_packer.make_can_msg_safety(
|
||||
"DRIVER_INPUT", VOLVO_PARTY_BUS, {"STEERING_DRIVER_INPUT": value})
|
||||
|
||||
for value in (2, 3, 5, 6, 20, -20):
|
||||
for value in (2, 3, 5):
|
||||
self._rx(driver_input_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._rx(driver_input_msg(value))
|
||||
self.assertTrue(self.safety.get_controls_allowed(), f"unexpected safety disengage at {value=}")
|
||||
self.assertFalse(self.safety.get_steering_disengage_prev())
|
||||
self.assertTrue(self.safety.get_controls_allowed(), f"unexpected disengage at {value=}")
|
||||
|
||||
self._rx(driver_input_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._rx(driver_input_msg(6))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# ---- Volvo-specific consistency tests ----
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-c03d06b4-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-26ce46ba-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user