openpilot v0.7.3 release
parent
b3e67e28e2
commit
332cb82886
585
.pylintrc
585
.pylintrc
|
@ -1,585 +0,0 @@
|
|||
[MASTER]
|
||||
|
||||
# A comma-separated list of package or module names from where C extensions may
|
||||
# be loaded. Extensions are loading into the active Python interpreter and may
|
||||
# run arbitrary code
|
||||
extension-pkg-whitelist=scipy
|
||||
|
||||
# Add files or directories to the blacklist. They should be base names, not
|
||||
# paths.
|
||||
ignore=CVS
|
||||
|
||||
# Add files or directories matching the regex patterns to the blacklist. The
|
||||
# regex matches against base names, not paths.
|
||||
ignore-patterns=
|
||||
|
||||
# Python code to execute, usually for sys.path manipulation such as
|
||||
# pygtk.require().
|
||||
#init-hook=
|
||||
|
||||
# Use multiple processes to speed up Pylint.
|
||||
jobs=4
|
||||
|
||||
# List of plugins (as comma separated values of python modules names) to load,
|
||||
# usually to register additional checkers.
|
||||
load-plugins=
|
||||
|
||||
# Pickle collected data for later comparisons.
|
||||
persistent=yes
|
||||
|
||||
# Specify a configuration file.
|
||||
#rcfile=
|
||||
|
||||
# When enabled, pylint would attempt to guess common misconfiguration and emit
|
||||
# user-friendly hints instead of false-positive error messages
|
||||
suggestion-mode=yes
|
||||
|
||||
# Allow loading of arbitrary C extensions. Extensions are imported into the
|
||||
# active Python interpreter and may run arbitrary code.
|
||||
unsafe-load-any-extension=no
|
||||
|
||||
|
||||
[MESSAGES CONTROL]
|
||||
|
||||
# Only show warnings with the listed confidence levels. Leave empty to show
|
||||
# all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED
|
||||
confidence=
|
||||
|
||||
# Disable the message, report, category or checker with the given id(s). You
|
||||
# can either give multiple identifiers separated by comma (,) or put this
|
||||
# option multiple times (only on the command line, not in the configuration
|
||||
# file where it should appear only once).You can also use "--disable=all" to
|
||||
# disable everything first and then reenable specific checks. For example, if
|
||||
# you want to run only the similarities checker, you can use "--disable=all
|
||||
# --enable=similarities". If you want to run only the classes checker, but have
|
||||
# no Warning level messages displayed, use"--disable=all --enable=classes
|
||||
# --disable=W"
|
||||
disable=print-statement,
|
||||
parameter-unpacking,
|
||||
unpacking-in-except,
|
||||
old-raise-syntax,
|
||||
backtick,
|
||||
long-suffix,
|
||||
old-ne-operator,
|
||||
old-octal-literal,
|
||||
import-star-module-level,
|
||||
non-ascii-bytes-literal,
|
||||
raw-checker-failed,
|
||||
bad-inline-option,
|
||||
locally-disabled,
|
||||
locally-enabled,
|
||||
file-ignored,
|
||||
suppressed-message,
|
||||
useless-suppression,
|
||||
deprecated-pragma,
|
||||
apply-builtin,
|
||||
basestring-builtin,
|
||||
buffer-builtin,
|
||||
cmp-builtin,
|
||||
coerce-builtin,
|
||||
execfile-builtin,
|
||||
file-builtin,
|
||||
long-builtin,
|
||||
raw_input-builtin,
|
||||
reduce-builtin,
|
||||
standarderror-builtin,
|
||||
unicode-builtin,
|
||||
xrange-builtin,
|
||||
coerce-method,
|
||||
delslice-method,
|
||||
getslice-method,
|
||||
setslice-method,
|
||||
no-absolute-import,
|
||||
old-division,
|
||||
dict-iter-method,
|
||||
dict-view-method,
|
||||
next-method-called,
|
||||
metaclass-assignment,
|
||||
indexing-exception,
|
||||
raising-string,
|
||||
reload-builtin,
|
||||
oct-method,
|
||||
hex-method,
|
||||
nonzero-method,
|
||||
cmp-method,
|
||||
input-builtin,
|
||||
round-builtin,
|
||||
intern-builtin,
|
||||
unichr-builtin,
|
||||
map-builtin-not-iterating,
|
||||
zip-builtin-not-iterating,
|
||||
range-builtin-not-iterating,
|
||||
filter-builtin-not-iterating,
|
||||
using-cmp-argument,
|
||||
eq-without-hash,
|
||||
div-method,
|
||||
idiv-method,
|
||||
rdiv-method,
|
||||
exception-message-attribute,
|
||||
invalid-str-codec,
|
||||
sys-max-int,
|
||||
bad-python3-import,
|
||||
deprecated-string-function,
|
||||
deprecated-str-translate-call,
|
||||
deprecated-itertools-function,
|
||||
deprecated-types-field,
|
||||
next-method-defined,
|
||||
dict-items-not-iterating,
|
||||
dict-keys-not-iterating,
|
||||
dict-values-not-iterating,
|
||||
bad-indentation,
|
||||
line-too-long,
|
||||
missing-docstring,
|
||||
multiple-statements,
|
||||
bad-continuation,
|
||||
invalid-name,
|
||||
too-many-arguments,
|
||||
too-many-locals,
|
||||
superfluous-parens,
|
||||
bad-whitespace,
|
||||
too-many-instance-attributes,
|
||||
wrong-import-position,
|
||||
ungrouped-imports,
|
||||
wrong-import-order,
|
||||
protected-access,
|
||||
trailing-whitespace,
|
||||
too-many-branches,
|
||||
too-few-public-methods,
|
||||
too-many-statements,
|
||||
trailing-newlines,
|
||||
attribute-defined-outside-init,
|
||||
too-many-return-statements,
|
||||
too-many-public-methods,
|
||||
unused-argument,
|
||||
old-style-class,
|
||||
no-init,
|
||||
len-as-condition,
|
||||
unneeded-not,
|
||||
no-self-use,
|
||||
multiple-imports,
|
||||
no-else-return,
|
||||
logging-not-lazy,
|
||||
fixme,
|
||||
redefined-outer-name,
|
||||
unused-variable,
|
||||
unsubscriptable-object,
|
||||
expression-not-assigned,
|
||||
too-many-boolean-expressions,
|
||||
consider-using-ternary,
|
||||
invalid-unary-operand-type,
|
||||
relative-import,
|
||||
deprecated-lambda
|
||||
|
||||
|
||||
# Enable the message, report, category or checker with the given id(s). You can
|
||||
# either give multiple identifier separated by comma (,) or put this option
|
||||
# multiple time (only on the command line, not in the configuration file where
|
||||
# it should appear only once). See also the "--disable" option for examples.
|
||||
enable=c-extension-no-member
|
||||
|
||||
|
||||
[REPORTS]
|
||||
|
||||
# Python expression which should return a note less than 10 (10 is the highest
|
||||
# note). You have access to the variables errors warning, statement which
|
||||
# respectively contain the number of errors / warnings messages and the total
|
||||
# number of statements analyzed. This is used by the global evaluation report
|
||||
# (RP0004).
|
||||
evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10)
|
||||
|
||||
# Template used to display messages. This is a python new-style format string
|
||||
# used to format the message information. See doc for all details
|
||||
#msg-template=
|
||||
|
||||
# Set the output format. Available formats are text, parseable, colorized, json
|
||||
# and msvs (visual studio).You can also give a reporter class, eg
|
||||
# mypackage.mymodule.MyReporterClass.
|
||||
output-format=text
|
||||
|
||||
# Tells whether to display a full report or only the messages
|
||||
reports=no
|
||||
|
||||
# Activate the evaluation score.
|
||||
score=yes
|
||||
|
||||
|
||||
[REFACTORING]
|
||||
|
||||
# Maximum number of nested blocks for function / method body
|
||||
max-nested-blocks=5
|
||||
|
||||
# Complete name of functions that never returns. When checking for
|
||||
# inconsistent-return-statements if a never returning function is called then
|
||||
# it will be considered as an explicit return statement and no message will be
|
||||
# printed.
|
||||
never-returning-functions=optparse.Values,sys.exit
|
||||
|
||||
|
||||
[LOGGING]
|
||||
|
||||
# Logging modules to check that the string format arguments are in logging
|
||||
# function parameter format
|
||||
logging-modules=logging
|
||||
|
||||
|
||||
[SPELLING]
|
||||
|
||||
# Limits count of emitted suggestions for spelling mistakes
|
||||
max-spelling-suggestions=4
|
||||
|
||||
# Spelling dictionary name. Available dictionaries: none. To make it working
|
||||
# install python-enchant package.
|
||||
spelling-dict=
|
||||
|
||||
# List of comma separated words that should not be checked.
|
||||
spelling-ignore-words=
|
||||
|
||||
# A path to a file that contains private dictionary; one word per line.
|
||||
spelling-private-dict-file=
|
||||
|
||||
# Tells whether to store unknown words to indicated private dictionary in
|
||||
# --spelling-private-dict-file option instead of raising a message.
|
||||
spelling-store-unknown-words=no
|
||||
|
||||
|
||||
[MISCELLANEOUS]
|
||||
|
||||
# List of note tags to take in consideration, separated by a comma.
|
||||
notes=FIXME,
|
||||
XXX,
|
||||
TODO
|
||||
|
||||
|
||||
[SIMILARITIES]
|
||||
|
||||
# Ignore comments when computing similarities.
|
||||
ignore-comments=yes
|
||||
|
||||
# Ignore docstrings when computing similarities.
|
||||
ignore-docstrings=yes
|
||||
|
||||
# Ignore imports when computing similarities.
|
||||
ignore-imports=no
|
||||
|
||||
# Minimum lines number of a similarity.
|
||||
min-similarity-lines=4
|
||||
|
||||
|
||||
[TYPECHECK]
|
||||
|
||||
# List of decorators that produce context managers, such as
|
||||
# contextlib.contextmanager. Add to this list to register other decorators that
|
||||
# produce valid context managers.
|
||||
contextmanager-decorators=contextlib.contextmanager
|
||||
|
||||
# List of members which are set dynamically and missed by pylint inference
|
||||
# system, and so shouldn't trigger E1101 when accessed. Python regular
|
||||
# expressions are accepted.
|
||||
generated-members=capnp.* cereal.* pygame.* zmq.* setproctitle.* smbus2.* usb1.* serial.* cv2.*
|
||||
|
||||
# Tells whether missing members accessed in mixin class should be ignored. A
|
||||
# mixin class is detected if its name ends with "mixin" (case insensitive).
|
||||
ignore-mixin-members=yes
|
||||
|
||||
# This flag controls whether pylint should warn about no-member and similar
|
||||
# checks whenever an opaque object is returned when inferring. The inference
|
||||
# can return multiple potential results while evaluating a Python object, but
|
||||
# some branches might not be evaluated, which results in partial inference. In
|
||||
# that case, it might be useful to still emit no-member and other checks for
|
||||
# the rest of the inferred objects.
|
||||
ignore-on-opaque-inference=yes
|
||||
|
||||
# List of class names for which member attributes should not be checked (useful
|
||||
# for classes with dynamically set attributes). This supports the use of
|
||||
# qualified names.
|
||||
ignored-classes=optparse.Values,thread._local,_thread._local
|
||||
|
||||
# List of module names for which member attributes should not be checked
|
||||
# (useful for modules/projects where namespaces are manipulated during runtime
|
||||
# and thus existing member attributes cannot be deduced by static analysis. It
|
||||
# supports qualified module names, as well as Unix pattern matching.
|
||||
ignored-modules=flask setproctitle usb1 flask.ext.socketio smbus2 usb1.*
|
||||
|
||||
# Show a hint with possible names when a member name was not found. The aspect
|
||||
# of finding the hint is based on edit distance.
|
||||
missing-member-hint=yes
|
||||
|
||||
# The minimum edit distance a name should have in order to be considered a
|
||||
# similar match for a missing member name.
|
||||
missing-member-hint-distance=1
|
||||
|
||||
# The total number of similar names that should be taken in consideration when
|
||||
# showing a hint for a missing member.
|
||||
missing-member-max-choices=1
|
||||
|
||||
|
||||
[VARIABLES]
|
||||
|
||||
# List of additional names supposed to be defined in builtins. Remember that
|
||||
# you should avoid to define new builtins when possible.
|
||||
additional-builtins=
|
||||
|
||||
# Tells whether unused global variables should be treated as a violation.
|
||||
allow-global-unused-variables=yes
|
||||
|
||||
# List of strings which can identify a callback function by name. A callback
|
||||
# name must start or end with one of those strings.
|
||||
callbacks=cb_,
|
||||
_cb
|
||||
|
||||
# A regular expression matching the name of dummy variables (i.e. expectedly
|
||||
# not used).
|
||||
dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_
|
||||
|
||||
# Argument names that match this expression will be ignored. Default to name
|
||||
# with leading underscore
|
||||
ignored-argument-names=_.*|^ignored_|^unused_
|
||||
|
||||
# Tells whether we should check for unused import in __init__ files.
|
||||
init-import=no
|
||||
|
||||
# List of qualified module names which can have objects that can redefine
|
||||
# builtins.
|
||||
redefining-builtins-modules=six.moves,past.builtins,future.builtins
|
||||
|
||||
|
||||
[FORMAT]
|
||||
|
||||
# Expected format of line ending, e.g. empty (any line ending), LF or CRLF.
|
||||
expected-line-ending-format=
|
||||
|
||||
# Regexp for a line that is allowed to be longer than the limit.
|
||||
ignore-long-lines=^\s*(# )?<?https?://\S+>?$
|
||||
|
||||
# Number of spaces of indent required inside a hanging or continued line.
|
||||
indent-after-paren=4
|
||||
|
||||
# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1
|
||||
# tab).
|
||||
indent-string=' '
|
||||
|
||||
# Maximum number of characters on a single line.
|
||||
max-line-length=100
|
||||
|
||||
# Maximum number of lines in a module
|
||||
max-module-lines=1000
|
||||
|
||||
# List of optional constructs for which whitespace checking is disabled. `dict-
|
||||
# separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}.
|
||||
# `trailing-comma` allows a space between comma and closing bracket: (a, ).
|
||||
# `empty-line` allows space-only lines.
|
||||
no-space-check=trailing-comma,
|
||||
dict-separator
|
||||
|
||||
# Allow the body of a class to be on the same line as the declaration if body
|
||||
# contains single statement.
|
||||
single-line-class-stmt=no
|
||||
|
||||
# Allow the body of an if to be on the same line as the test if there is no
|
||||
# else.
|
||||
single-line-if-stmt=no
|
||||
|
||||
|
||||
[BASIC]
|
||||
|
||||
# Naming style matching correct argument names
|
||||
argument-naming-style=snake_case
|
||||
|
||||
# Regular expression matching correct argument names. Overrides argument-
|
||||
# naming-style
|
||||
#argument-rgx=
|
||||
|
||||
# Naming style matching correct attribute names
|
||||
attr-naming-style=snake_case
|
||||
|
||||
# Regular expression matching correct attribute names. Overrides attr-naming-
|
||||
# style
|
||||
#attr-rgx=
|
||||
|
||||
# Bad variable names which should always be refused, separated by a comma
|
||||
bad-names=foo,
|
||||
bar,
|
||||
baz,
|
||||
toto,
|
||||
tutu,
|
||||
tata
|
||||
|
||||
# Naming style matching correct class attribute names
|
||||
class-attribute-naming-style=any
|
||||
|
||||
# Regular expression matching correct class attribute names. Overrides class-
|
||||
# attribute-naming-style
|
||||
#class-attribute-rgx=
|
||||
|
||||
# Naming style matching correct class names
|
||||
class-naming-style=PascalCase
|
||||
|
||||
# Regular expression matching correct class names. Overrides class-naming-style
|
||||
#class-rgx=
|
||||
|
||||
# Naming style matching correct constant names
|
||||
const-naming-style=UPPER_CASE
|
||||
|
||||
# Regular expression matching correct constant names. Overrides const-naming-
|
||||
# style
|
||||
#const-rgx=
|
||||
|
||||
# Minimum line length for functions/classes that require docstrings, shorter
|
||||
# ones are exempt.
|
||||
docstring-min-length=-1
|
||||
|
||||
# Naming style matching correct function names
|
||||
function-naming-style=snake_case
|
||||
|
||||
# Regular expression matching correct function names. Overrides function-
|
||||
# naming-style
|
||||
#function-rgx=
|
||||
|
||||
# Good variable names which should always be accepted, separated by a comma
|
||||
good-names=i,
|
||||
j,
|
||||
k,
|
||||
ex,
|
||||
Run,
|
||||
_
|
||||
|
||||
# Include a hint for the correct naming format with invalid-name
|
||||
include-naming-hint=no
|
||||
|
||||
# Naming style matching correct inline iteration names
|
||||
inlinevar-naming-style=any
|
||||
|
||||
# Regular expression matching correct inline iteration names. Overrides
|
||||
# inlinevar-naming-style
|
||||
#inlinevar-rgx=
|
||||
|
||||
# Naming style matching correct method names
|
||||
method-naming-style=snake_case
|
||||
|
||||
# Regular expression matching correct method names. Overrides method-naming-
|
||||
# style
|
||||
#method-rgx=
|
||||
|
||||
# Naming style matching correct module names
|
||||
module-naming-style=snake_case
|
||||
|
||||
# Regular expression matching correct module names. Overrides module-naming-
|
||||
# style
|
||||
#module-rgx=
|
||||
|
||||
# Colon-delimited sets of names that determine each other's naming style when
|
||||
# the name regexes allow several styles.
|
||||
name-group=
|
||||
|
||||
# Regular expression which should only match function or class names that do
|
||||
# not require a docstring.
|
||||
no-docstring-rgx=^_
|
||||
|
||||
# List of decorators that produce properties, such as abc.abstractproperty. Add
|
||||
# to this list to register other decorators that produce valid properties.
|
||||
property-classes=abc.abstractproperty
|
||||
|
||||
# Naming style matching correct variable names
|
||||
variable-naming-style=snake_case
|
||||
|
||||
# Regular expression matching correct variable names. Overrides variable-
|
||||
# naming-style
|
||||
#variable-rgx=
|
||||
|
||||
|
||||
[DESIGN]
|
||||
|
||||
# Maximum number of arguments for function / method
|
||||
max-args=5
|
||||
|
||||
# Maximum number of attributes for a class (see R0902).
|
||||
max-attributes=7
|
||||
|
||||
# Maximum number of boolean expressions in a if statement
|
||||
max-bool-expr=5
|
||||
|
||||
# Maximum number of branch for function / method body
|
||||
max-branches=12
|
||||
|
||||
# Maximum number of locals for function / method body
|
||||
max-locals=15
|
||||
|
||||
# Maximum number of parents for a class (see R0901).
|
||||
max-parents=7
|
||||
|
||||
# Maximum number of public methods for a class (see R0904).
|
||||
max-public-methods=20
|
||||
|
||||
# Maximum number of return / yield for function / method body
|
||||
max-returns=6
|
||||
|
||||
# Maximum number of statements in function / method body
|
||||
max-statements=50
|
||||
|
||||
# Minimum number of public methods for a class (see R0903).
|
||||
min-public-methods=2
|
||||
|
||||
|
||||
[CLASSES]
|
||||
|
||||
# List of method names used to declare (i.e. assign) instance attributes.
|
||||
defining-attr-methods=__init__,
|
||||
__new__,
|
||||
setUp
|
||||
|
||||
# List of member names, which should be excluded from the protected access
|
||||
# warning.
|
||||
exclude-protected=_asdict,
|
||||
_fields,
|
||||
_replace,
|
||||
_source,
|
||||
_make
|
||||
|
||||
# List of valid names for the first argument in a class method.
|
||||
valid-classmethod-first-arg=cls
|
||||
|
||||
# List of valid names for the first argument in a metaclass class method.
|
||||
valid-metaclass-classmethod-first-arg=mcs
|
||||
|
||||
|
||||
[IMPORTS]
|
||||
|
||||
# Allow wildcard imports from modules that define __all__.
|
||||
allow-wildcard-with-all=no
|
||||
|
||||
# Analyse import fallback blocks. This can be used to support both Python 2 and
|
||||
# 3 compatible code, which means that the block might have code that exists
|
||||
# only in one or another interpreter, leading to false positives when analysed.
|
||||
analyse-fallback-blocks=no
|
||||
|
||||
# Deprecated modules which should not be used, separated by a comma
|
||||
deprecated-modules=regsub,
|
||||
TERMIOS,
|
||||
Bastion,
|
||||
rexec
|
||||
|
||||
# Create a graph of external dependencies in the given file (report RP0402 must
|
||||
# not be disabled)
|
||||
ext-import-graph=
|
||||
|
||||
# Create a graph of every (i.e. internal and external) dependencies in the
|
||||
# given file (report RP0402 must not be disabled)
|
||||
import-graph=
|
||||
|
||||
# Create a graph of internal dependencies in the given file (report RP0402 must
|
||||
# not be disabled)
|
||||
int-import-graph=
|
||||
|
||||
# Force import order to recognize a module as part of the standard
|
||||
# compatibility libraries.
|
||||
known-standard-library=
|
||||
|
||||
# Force import order to recognize a module as part of a third party library.
|
||||
known-third-party=enchant
|
||||
|
||||
|
||||
[EXCEPTIONS]
|
||||
|
||||
# Exceptions that will emit a warning when being caught. Defaults to
|
||||
# "Exception"
|
||||
overgeneral-exceptions=Exception
|
|
@ -1,7 +0,0 @@
|
|||
sudo: required
|
||||
|
||||
services:
|
||||
- docker
|
||||
|
||||
script:
|
||||
- ./run_docker_tests.sh
|
|
@ -96,6 +96,7 @@ Supported Cars
|
|||
| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph |
|
||||
| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph |
|
||||
| Lexus | NX Hybrid 2018 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2016-17 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Lexus | RX 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
|
@ -113,6 +114,7 @@ Supported Cars
|
|||
| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Highlander 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017-20 | All | Stock<sup>4</sup>| 0mph | 0mph |
|
||||
|
|
|
@ -1,3 +1,11 @@
|
|||
Version 0.7.3 (2020-02-21)
|
||||
========================
|
||||
* Improved autofocus for road facing camera
|
||||
* Support for 2020 Highlander thanks to che220!
|
||||
* Support for 2018 Lexus NX 300h thanks to kengggg!
|
||||
* Speed up ECU firmware query
|
||||
* Fix bug where manager would sometimes hang after shutting down the car
|
||||
|
||||
Version 0.7.2 (2020-02-07)
|
||||
========================
|
||||
* ECU firmware version based fingerprinting for Honda & Toyota
|
||||
|
|
|
@ -228,5 +228,6 @@ if arch == "aarch64":
|
|||
SConscript(['selfdrive/clocksd/SConscript'])
|
||||
|
||||
SConscript(['selfdrive/locationd/SConscript'])
|
||||
SConscript(['selfdrive/locationd/kalman/SConscript'])
|
||||
|
||||
# TODO: finish cereal, dbcbuilder, MPC
|
||||
|
|
Binary file not shown.
|
@ -81,6 +81,7 @@ keys = {
|
|||
"LiveParameters": [TxType.PERSISTENT],
|
||||
"LongitudinalControl": [TxType.PERSISTENT],
|
||||
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
|
||||
"LaneChangeEnabled": [TxType.PERSISTENT],
|
||||
"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
|
@ -93,6 +94,7 @@ keys = {
|
|||
"TermsVersion": [TxType.PERSISTENT],
|
||||
"TrainingVersion": [TxType.PERSISTENT],
|
||||
"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Version": [TxType.PERSISTENT],
|
||||
"Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START],
|
||||
|
|
|
@ -1710,6 +1710,9 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
|
|||
int good_count = 0;
|
||||
int16_t max_focus = -32767;
|
||||
int avg_focus = 0;
|
||||
// force to max if not able to determine focus for long
|
||||
const int patience_cnt = 100;
|
||||
static int nan_cnt = 0;
|
||||
|
||||
/*printf("FOCUS: ");
|
||||
for (int i = 0; i < 0x10; i++) {
|
||||
|
@ -1717,42 +1720,56 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
|
|||
}*/
|
||||
|
||||
for (int i = 0; i < NUM_FOCUS; i++) {
|
||||
int doff = i*5+5;
|
||||
s->confidence[i] = d[doff];
|
||||
int16_t focus_t = (d[doff+1] << 3) | (d[doff+2] >> 5);
|
||||
if (focus_t >= 1024) focus_t = -(2048-focus_t);
|
||||
s->focus[i] = focus_t;
|
||||
//printf("%x->%d ", d[doff], focus_t);
|
||||
if (s->confidence[i] > 0x20) {
|
||||
int pd_idx = (i+1)*5;
|
||||
s->confidence[i] = d[pd_idx];
|
||||
int16_t focus_delta = d[pd_idx+1];
|
||||
if (focus_delta >= 128) focus_delta = - (256 - focus_delta);
|
||||
s->focus[i] = focus_delta;
|
||||
|
||||
if (s->confidence[i] > 64) {
|
||||
good_count++;
|
||||
max_focus = max(max_focus, s->focus[i]);
|
||||
avg_focus += s->focus[i];
|
||||
// printf("%d\n", s->focus[i]);
|
||||
}
|
||||
}
|
||||
|
||||
//printf("\n");
|
||||
if (good_count < 4) {
|
||||
if (good_count < 7) {
|
||||
s->focus_err = nan("");
|
||||
nan_cnt += 1;
|
||||
if (nan_cnt > patience_cnt) {
|
||||
s->focus_err = 16;
|
||||
nan_cnt = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
avg_focus /= good_count;
|
||||
|
||||
// outlier rejection
|
||||
if (abs(avg_focus - max_focus) > 200) {
|
||||
s->focus_err = nan("");
|
||||
return;
|
||||
if (abs(avg_focus - max_focus) > 32) {
|
||||
if (nan_cnt < patience_cnt) {
|
||||
s->focus_err = nan("");
|
||||
nan_cnt += 1;
|
||||
return;
|
||||
} else {
|
||||
s->focus_err = 16;
|
||||
// s->focus_err = max_focus*8.0;
|
||||
nan_cnt = 0;
|
||||
}
|
||||
} else {
|
||||
s->focus_err = avg_focus;
|
||||
nan_cnt = 0;
|
||||
}
|
||||
|
||||
s->focus_err = max_focus*1.0;
|
||||
// printf("fe=%f\n", s->focus_err);
|
||||
}
|
||||
|
||||
static void do_autofocus(CameraState *s) {
|
||||
// params for focus PI controller
|
||||
const float focus_kp = 0.005;
|
||||
// params for focus P controller
|
||||
const float focus_kp = 0.1;
|
||||
|
||||
float err = s->focus_err;
|
||||
float offset = 0;
|
||||
// don't allow big change
|
||||
err = clamp(err, -16, 16);
|
||||
float sag = (s->last_sag_acc_z/9.8) * 128;
|
||||
|
||||
const int dac_up = s->device == DEVICE_LP3? 634:456;
|
||||
|
@ -1776,6 +1793,7 @@ static void do_autofocus(CameraState *s) {
|
|||
LOGD(debug);*/
|
||||
|
||||
actuator_move(s, target);
|
||||
// printf("ltp=%f, clp=%d\n",s->lens_true_pos,s->cur_lens_pos);
|
||||
}
|
||||
|
||||
|
||||
|
@ -2165,6 +2183,9 @@ void cameras_run(DualCameraState *s) {
|
|||
} else {
|
||||
uint8_t *d = c->ss[buffer].bufs[buf_idx].addr;
|
||||
if (buffer == 1) {
|
||||
// FILE *df = fopen("/sdcard/focus_buf","wb");
|
||||
// fwrite(d, c->ss[buffer].bufs[buf_idx].len, sizeof(uint8_t), df);
|
||||
// fclose(df);
|
||||
parse_autofocus(c, d);
|
||||
}
|
||||
c->ss[buffer].qbuf_info[buf_idx].dirty_buf = 1;
|
||||
|
|
|
@ -133,6 +133,11 @@ def fingerprint(logcan, sendcan, has_relay):
|
|||
car_fingerprint = list(fw_candidates)[0]
|
||||
source = car.CarParams.FingerprintSource.fw
|
||||
|
||||
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
|
||||
if len(fixed_fingerprint):
|
||||
car_fingerprint = fixed_fingerprint
|
||||
source = car.CarParams.FingerprintSource.fixed
|
||||
|
||||
cloudlog.warning("fingerprinted %s", car_fingerprint)
|
||||
return car_fingerprint, finger, vin, car_fw, source
|
||||
|
||||
|
|
|
@ -35,14 +35,14 @@ class CarController():
|
|||
# steer torque
|
||||
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
|
||||
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
|
||||
CS.steer_torque_motor, SteerLimitParams)
|
||||
CS.out.steeringTorqueEps, SteerLimitParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message
|
||||
if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
|
||||
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
self.gone_fast_yet = True
|
||||
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020_HYBRID, CAR.JEEP_CHEROKEE_2019):
|
||||
if CS.v_ego < (CS.CP.minSteerSpeed - 3.0):
|
||||
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
|
||||
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
|
||||
lkas_active = moving_fast and enabled
|
||||
|
||||
|
@ -65,7 +65,7 @@ class CarController():
|
|||
if (self.ccframe % 25 == 0): # 0.25s period
|
||||
if (CS.lkas_car_model != -1):
|
||||
new_msg = create_lkas_hud(
|
||||
self.packer, CS.gear_shifter, lkas_active, hud_alert,
|
||||
self.packer, CS.out.gearShifter, lkas_active, hud_alert,
|
||||
self.hud_count, CS.lkas_car_model)
|
||||
can_sends.append(new_msg)
|
||||
self.hud_count += 1
|
||||
|
|
|
@ -1,14 +1,9 @@
|
|||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def parse_gear_shifter(can_gear):
|
||||
return {0x1: GearShifter.park, 0x2: GearShifter.reverse, 0x3: GearShifter.neutral,
|
||||
0x4: GearShifter.drive, 0x5: GearShifter.low}.get(can_gear, GearShifter.unknown)
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
|
@ -68,83 +63,61 @@ def get_camera_parser(CP):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["GEAR"]['PRNDL']
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER'])
|
||||
self.frame = int(cp.vl["EPS_STATUS"]['COUNTER'])
|
||||
|
||||
self.door_all_closed = not any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
|
||||
ret.doorOpen = any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RR']])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 1
|
||||
|
||||
ret.brakePressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
|
||||
ret.brake = 0
|
||||
ret.brakeLights = ret.brakePressed
|
||||
ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
|
||||
self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
|
||||
self.car_gas = self.pedal_gas
|
||||
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
|
||||
|
||||
self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
|
||||
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
|
||||
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
|
||||
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
|
||||
v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
|
||||
ret.wheelSpeeds.fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
|
||||
ret.wheelSpeeds.rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
|
||||
ret.wheelSpeeds.rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
|
||||
ret.wheelSpeeds.fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
|
||||
ret.vEgoRaw = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE']
|
||||
ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE']
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = not v_wheel > 0.001
|
||||
ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
|
||||
ret.cruiseState.available = ret.cruiseState.enabled # FIXME: for now same as enabled
|
||||
ret.cruiseState.speed = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH'] * CV.KPH_TO_MS
|
||||
|
||||
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
|
||||
self.gear_shifter = parse_gear_shifter(cp.vl['GEAR']['PRNDL'])
|
||||
self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
self.steer_torque_driver = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
|
||||
self.steer_torque_motor = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
|
||||
ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
|
||||
self.steer_error = steer_state == 4 or (steer_state == 0 and self.v_ego > self.CP.minSteerSpeed)
|
||||
self.steer_error = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
|
||||
|
||||
self.user_brake = 0
|
||||
self.brake_lights = self.brake_pressed
|
||||
self.v_cruise_pcm = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH']
|
||||
self.pcm_acc_status = self.main_on
|
||||
|
||||
self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
|
||||
ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
|
||||
|
||||
self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER']
|
||||
self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL']
|
||||
self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK']
|
||||
|
||||
return ret
|
||||
|
|
|
@ -20,6 +20,8 @@ class CarInterface(CarInterfaceBase):
|
|||
self.brake_pressed_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
self.low_speed_alert = False
|
||||
self.left_blinker_prev = False
|
||||
self.right_blinker_prev = False
|
||||
|
||||
# *** init the major players ***
|
||||
self.CS = CarState(CP)
|
||||
|
@ -113,78 +115,33 @@ class CarInterface(CarInterfaceBase):
|
|||
self.cp.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.cp, self.cp_cam)
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.aEgo = self.CS.a_ego
|
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
ret.gasPressed = self.CS.pedal_gas > 0
|
||||
|
||||
# brake pedal
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringRate = self.CS.angle_steers_rate
|
||||
|
||||
ret.steeringTorque = self.CS.steer_torque_driver
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = self.CS.main_on
|
||||
ret.cruiseState.speedOffset = 0.
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
if ret.leftBlinker != self.left_blinker_prev:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.leftBlinker
|
||||
be.pressed = self.CS.left_blinker_on != 0
|
||||
be.pressed = ret.leftBlinker != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
|
||||
if ret.rightBlinker != self.right_blinker_prev:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.rightBlinker
|
||||
be.pressed = self.CS.right_blinker_on != 0
|
||||
be.pressed = ret.rightBlinker != 0
|
||||
buttonEvents.append(be)
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
|
||||
|
||||
ret.genericToggle = self.CS.generic_toggle
|
||||
#ret.lkasCounter = self.CS.lkas_counter
|
||||
#ret.lkasCarModel = self.CS.lkas_car_model
|
||||
|
||||
# events
|
||||
events = []
|
||||
if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
|
||||
|
@ -195,7 +152,7 @@ class CarInterface(CarInterfaceBase):
|
|||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.main_on:
|
||||
if not ret.cruiseState.available:
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
|
@ -220,8 +177,13 @@ class CarInterface(CarInterfaceBase):
|
|||
self.gas_pressed_prev = ret.gasPressed
|
||||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
self.left_blinker_prev = ret.leftBlinker
|
||||
self.right_blinker_prev = ret.rightBlinker
|
||||
|
||||
return ret.as_reader()
|
||||
# copy back carState packet to CS
|
||||
self.CS.out = ret.as_reader()
|
||||
|
||||
return self.CS.out
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
|
|
|
@ -53,7 +53,10 @@ FINGERPRINTS = {
|
|||
# Based on 3c7ce223e3571b54|2019-05-11--20-16-14
|
||||
{
|
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8
|
||||
}
|
||||
},
|
||||
# Based on "d26bf42deb1910e7|2020-02-13--16-22-31"
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1262: 8, 1284: 8, 1568: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2023: 8, 2024: 8, 2026: 8, 2031: 8
|
||||
},
|
||||
],
|
||||
CAR.PACIFICA_2020_HYBRID: [
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 2015: 8, 2016: 8, 2024: 8},
|
||||
|
|
|
@ -1,11 +1,12 @@
|
|||
import os
|
||||
from common.basedir import BASEDIR
|
||||
|
||||
def get_attr_from_cars(attr):
|
||||
|
||||
def get_attr_from_cars(attr, result=dict):
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car models
|
||||
# - values are attr values from all car folders
|
||||
result = {}
|
||||
result = result()
|
||||
|
||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||
try:
|
||||
|
@ -16,8 +17,11 @@ def get_attr_from_cars(attr):
|
|||
else:
|
||||
continue
|
||||
|
||||
for f, v in attr_values.items():
|
||||
result[f] = v
|
||||
if isinstance(attr_values, dict):
|
||||
for f, v in attr_values.items():
|
||||
result[f] = v
|
||||
elif isinstance(attr_values, list):
|
||||
result += attr_values
|
||||
|
||||
except (ImportError, IOError):
|
||||
pass
|
||||
|
@ -25,20 +29,9 @@ def get_attr_from_cars(attr):
|
|||
return result
|
||||
|
||||
|
||||
def get_fw_versions_list():
|
||||
return get_attr_from_cars('FW_VERSIONS')
|
||||
|
||||
|
||||
def get_fingerprint_list():
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car models for which we have a fingerprint
|
||||
# - values are lists dicts of messages that constitute the unique
|
||||
# CAN fingerprint of each car model and all its variants
|
||||
return get_attr_from_cars('FINGERPRINTS')
|
||||
|
||||
|
||||
FW_VERSIONS = get_fw_versions_list()
|
||||
_FINGERPRINTS = get_fingerprint_list()
|
||||
FW_VERSIONS = get_attr_from_cars('FW_VERSIONS')
|
||||
_FINGERPRINTS = get_attr_from_cars('FINGERPRINTS')
|
||||
IGNORED_FINGERPRINTS = get_attr_from_cars('IGNORED_FINGERPRINTS', list)
|
||||
|
||||
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
||||
|
||||
|
@ -61,6 +54,9 @@ def eliminate_incompatible_cars(msg, candidate_cars):
|
|||
compatible_cars = []
|
||||
|
||||
for car_name in candidate_cars:
|
||||
if car_name in IGNORED_FINGERPRINTS:
|
||||
continue
|
||||
|
||||
car_fingerprints = _FINGERPRINTS[car_name]
|
||||
|
||||
for fingerprint in car_fingerprints:
|
||||
|
|
|
@ -33,27 +33,27 @@ class CarController():
|
|||
|
||||
if (frame % 3) == 0:
|
||||
|
||||
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego)
|
||||
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.out.vEgo)
|
||||
|
||||
# The use of the toggle below is handy for trying out the various LKAS modes
|
||||
if TOGGLE_DEBUG:
|
||||
self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last)
|
||||
self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last)
|
||||
self.lkas_action &= 0xf
|
||||
else:
|
||||
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
|
||||
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
|
||||
CS.lkas_state, CS.angle_steers, curvature, self.lkas_action))
|
||||
self.generic_toggle_last = CS.generic_toggle
|
||||
CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action))
|
||||
self.generic_toggle_last = CS.out.genericToggle
|
||||
|
||||
if (frame % 100) == 0:
|
||||
|
||||
can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
|
||||
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
|
||||
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
|
||||
(self.steer_alert_last != steer_alert):
|
||||
can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert))
|
||||
can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert))
|
||||
|
||||
if (frame % 200) == 0:
|
||||
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
|
||||
|
@ -81,7 +81,7 @@ class CarController():
|
|||
can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1))
|
||||
|
||||
self.enabled_last = enabled
|
||||
self.main_on_last = CS.main_on
|
||||
self.main_on_last = CS.out.cruiseState.available
|
||||
self.steer_alert_last = steer_alert
|
||||
|
||||
return can_sends
|
||||
|
|
|
@ -1,8 +1,9 @@
|
|||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from common.numpy_fast import mean
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.ford.values import DBC
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
WHEEL_RADIUS = 0.33
|
||||
|
||||
|
@ -32,57 +33,28 @@ def get_can_parser(CP):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def update(self, cp):
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = not v_wheel > 0.001
|
||||
|
||||
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
|
||||
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
|
||||
self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State']
|
||||
self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0
|
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl']
|
||||
ret = car.CarState.new_message()
|
||||
ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
|
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
|
||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
|
||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]['Cruise_State'] in [0, 3])
|
||||
ret.cruiseState.available = cp.vl["Cruise_Status"]['Cruise_State'] != 0
|
||||
ret.gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] / 100.
|
||||
ret.gasPressed = ret.gas > 1e-6
|
||||
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
|
||||
ret.brakeLights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
|
||||
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
|
||||
# TODO: we also need raw driver torque, needed for Assisted Lane Change
|
||||
self.steer_override = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
|
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl']
|
||||
self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl']
|
||||
self.user_gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl']
|
||||
self.brake_pressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
|
||||
self.brake_lights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
|
||||
self.generic_toggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
|
||||
|
||||
return ret
|
||||
|
|
|
@ -106,38 +106,10 @@ class CarInterface(CarInterfaceBase):
|
|||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp)
|
||||
|
||||
# create message
|
||||
ret = car.CarState.new_message()
|
||||
ret = self.CS.update(self.cp)
|
||||
|
||||
ret.canValid = self.cp.can_valid
|
||||
|
||||
# speeds
|
||||
ret.vEgo = self.CS.v_ego
|
||||
ret.vEgoRaw = self.CS.v_ego_raw
|
||||
ret.standstill = self.CS.standstill
|
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
|
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
|
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngle = self.CS.angle_steers
|
||||
ret.steeringPressed = self.CS.steer_override
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.user_gas / 100.
|
||||
ret.gasPressed = self.CS.user_gas > 0.0001
|
||||
ret.brakePressed = self.CS.brake_pressed
|
||||
ret.brakeLights = self.CS.brake_lights
|
||||
|
||||
ret.cruiseState.enabled = not (self.CS.pcm_acc_status in [0, 3])
|
||||
ret.cruiseState.speed = self.CS.v_cruise_pcm
|
||||
ret.cruiseState.available = self.CS.pcm_acc_status != 0
|
||||
|
||||
ret.genericToggle = self.CS.generic_toggle
|
||||
|
||||
# events
|
||||
events = []
|
||||
|
||||
|
@ -167,7 +139,9 @@ class CarInterface(CarInterfaceBase):
|
|||
self.brake_pressed_prev = ret.brakePressed
|
||||
self.cruise_enabled_prev = ret.cruiseState.enabled
|
||||
|
||||
return ret.as_reader()
|
||||
self.CS.out = ret.as_reader()
|
||||
|
||||
return self.CS.out
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
|
|
|
@ -123,9 +123,12 @@ def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False,
|
|||
ecu_types[a] = ecu_type
|
||||
|
||||
if sub_addr is None:
|
||||
parallel_addrs.append(a)
|
||||
if a not in parallel_addrs:
|
||||
parallel_addrs.append(a)
|
||||
else:
|
||||
addrs.append([a])
|
||||
if [a] not in addrs:
|
||||
addrs.append([a])
|
||||
|
||||
addrs.insert(0, parallel_addrs)
|
||||
|
||||
fw_versions = {}
|
||||
|
|
|
@ -1,9 +1,10 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.gm.values import DBC, CAR, \
|
||||
CruiseButtons, is_eps_status_ok, \
|
||||
STEER_THRESHOLD, SUPERCRUISE_CARS
|
||||
|
||||
|
@ -50,25 +51,11 @@ def get_powertrain_can_parser(CP, canbus):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP, canbus):
|
||||
self.CP = CP
|
||||
# initialize can parser
|
||||
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.cruise_buttons = CruiseButtons.UNPRESS
|
||||
self.left_blinker_on = False
|
||||
self.prev_left_blinker_on = False
|
||||
self.right_blinker_on = False
|
||||
self.prev_right_blinker_on = False
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
|
||||
|
||||
def update(self, pt_cp):
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
|
@ -78,20 +65,12 @@ class CarState():
|
|||
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
|
||||
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
self.standstill = self.v_ego_raw < 0.01
|
||||
|
||||
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
|
||||
self.gear_shifter = parse_gear_shifter(pt_cp.vl["ECMPRDNL"]['PRNDL'])
|
||||
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
|
||||
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
|
||||
|
||||
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
|
||||
|
|
|
@ -67,8 +67,8 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_f
|
|||
else:
|
||||
mode = 0xa
|
||||
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
# TODO: this is to have GM bringing the car to complete stop,
|
||||
# but currently it conflicts with OP controls, so turned off.
|
||||
#elif near_stop:
|
||||
|
|
|
@ -29,7 +29,7 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
# *** init the major players ***
|
||||
canbus = CanBus()
|
||||
self.CS = CarState(CP, canbus)
|
||||
self.CS = CarState(CP)
|
||||
self.VM = VehicleModel(CP)
|
||||
self.pt_cp = get_powertrain_can_parser(CP, canbus)
|
||||
self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis']
|
||||
|
@ -64,6 +64,13 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
|
||||
# Start with a baseline lateral tuning for all GM vehicles. Override tuning as needed in each model section below.
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
|
||||
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
ret.steerRateCost = 1.0
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
|
||||
if candidate == CAR.VOLT:
|
||||
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
|
@ -141,11 +148,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
# same tuning for Volt and CT6 for now
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
|
||||
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
|
||||
ret.steerMaxBP = [0.] # m/s
|
||||
ret.steerMaxV = [1.]
|
||||
ret.gasMaxBP = [0.]
|
||||
|
@ -163,8 +165,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.stoppingControl = True
|
||||
ret.startAccel = 0.8
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
ret.steerRateCost = 1.0
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
@ -237,7 +237,7 @@ class CarInterface(CarInterfaceBase):
|
|||
be.pressed = self.CS.right_blinker_on
|
||||
buttonEvents.append(be)
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = ButtonType.unknown
|
||||
if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
|
||||
|
|
|
@ -14,6 +14,7 @@ class CAR:
|
|||
SUPERCRUISE_CARS = [CAR.CADILLAC_CT6]
|
||||
|
||||
class CruiseButtons:
|
||||
INIT = 0
|
||||
UNPRESS = 1
|
||||
RES_ACCEL = 2
|
||||
DECEL_SET = 3
|
||||
|
@ -34,18 +35,6 @@ def is_eps_status_ok(eps_status, car_fingerprint):
|
|||
valid_eps_status += [0, 1]
|
||||
return eps_status in valid_eps_status
|
||||
|
||||
def parse_gear_shifter(can_gear):
|
||||
if can_gear == 0:
|
||||
return car.CarState.GearShifter.park
|
||||
elif can_gear == 1:
|
||||
return car.CarState.GearShifter.neutral
|
||||
elif can_gear == 2:
|
||||
return car.CarState.GearShifter.drive
|
||||
elif can_gear == 3:
|
||||
return car.CarState.GearShifter.reverse
|
||||
else:
|
||||
return car.CarState.GearShifter.unknown
|
||||
|
||||
FINGERPRINTS = {
|
||||
# Astra BK MY17, ASCM unplugged
|
||||
CAR.HOLDEN_ASTRA: [{
|
||||
|
|
|
@ -1,19 +1,11 @@
|
|||
from cereal import car
|
||||
from collections import defaultdict
|
||||
from common.numpy_fast import interp
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def parse_gear_shifter(gear):
|
||||
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
|
||||
'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low}.get(gear, GearShifter.unknown)
|
||||
|
||||
|
||||
def calc_cruise_offset(offset, speed):
|
||||
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
|
||||
# constraints to solve for _K0, _K1, _K2 are:
|
||||
|
@ -188,38 +180,21 @@ def get_cam_can_parser(CP):
|
|||
bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
|
||||
|
||||
class CarState():
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
|
||||
self.user_gas, self.user_gas_pressed = 0., 0
|
||||
self.brake_switch_prev = 0
|
||||
self.brake_switch_ts = 0
|
||||
|
||||
self.cruise_buttons = 0
|
||||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
self.blinker_on = 0
|
||||
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
|
||||
self.cruise_mode = 0
|
||||
self.stopped = 0
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
# car params
|
||||
|
@ -229,8 +204,6 @@ class CarState():
|
|||
# update prevs, update must run once per loop
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.prev_cruise_setting = self.cruise_setting
|
||||
self.prev_blinker_on = self.blinker_on
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
|
@ -260,7 +233,6 @@ class CarState():
|
|||
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
|
||||
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
|
||||
|
@ -270,16 +242,10 @@ class CarState():
|
|||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
|
||||
self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
|
||||
self.v_weight * v_wheel
|
||||
|
||||
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[speed], [0.0]]
|
||||
|
||||
self.v_ego_raw = speed
|
||||
v_ego_x = self.v_ego_kf.update(speed)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
|
||||
# this is a hack for the interceptor. This is now only used in the simulation
|
||||
# TODO: Replace tests by toyota so this can go away
|
||||
|
@ -310,7 +276,7 @@ class CarState():
|
|||
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
|
||||
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
|
||||
self.gear_shifter = parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
|
||||
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
|
||||
# crv doesn't include cruise control
|
||||
|
|
|
@ -132,11 +132,12 @@ FW_VERSIONS = {
|
|||
b'37805-6A0-A640\x00\x00',
|
||||
b'37805-6B2-A550\x00\x00',
|
||||
b'37805-6B2-A650\x00\x00',
|
||||
b'37805-6B2-A660\x00\x00',
|
||||
b'37805-6B2-M520\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da0bf1, None): [b'54008-TVC-A910\x00\x00'],
|
||||
(Ecu.unknown, 0x18da1ef1, None): [b'28102-6B8-A560\x00\x00', b'28102-6B8-M520\x00\x00'],
|
||||
(Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00'],
|
||||
(Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00', b'46114-TVA-A080\x00\x00'],
|
||||
(Ecu.unknown, 0x18da28f1, None): [b'57114-TVA-C050\x00\x00'],
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TVA-A150\x00\x00',
|
||||
|
@ -153,7 +154,11 @@ FW_VERSIONS = {
|
|||
b'78109-TVC-M510\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'],
|
||||
(Ecu.unknown, 0x18dab0f1, None): [b'36802-TVA-A160\x00\x00', b'36802-TWA-A070\x00\x00'],
|
||||
(Ecu.unknown, 0x18dab0f1, None): [
|
||||
b'36802-TVA-A160\x00\x00',
|
||||
b'36802-TVA-A170\x00\x00',
|
||||
b'36802-TWA-A070\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00', b'36161-TWA-A070\x00\x00'],
|
||||
(Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A010\x00\x00'],
|
||||
},
|
||||
|
@ -224,6 +229,7 @@ FW_VERSIONS = {
|
|||
b'37805-5AA-A670\x00\x00',
|
||||
b'37805-5AA-A680\x00\x00',
|
||||
b'37805-5AA-A810\x00\x00',
|
||||
b'37805-5AA-C820\x00\x00',
|
||||
b'37805-5AA-L660\x00\x00',
|
||||
b'37805-5AJ-A610\x00\x00',
|
||||
b'37805-5BA-A510\x00\x00',
|
||||
|
@ -236,6 +242,7 @@ FW_VERSIONS = {
|
|||
b'28101-5CG-A070\x00\x00',
|
||||
b'28101-5CG-A080\x00\x00',
|
||||
b'28101-5CG-A810\x00\x00',
|
||||
b'28101-5CG-A820\x00\x00',
|
||||
b'28101-5DJ-A040\x00\x00',
|
||||
b'28101-5DJ-A060\x00\x00',
|
||||
b'28101-5DJ-A510\x00\x00',
|
||||
|
@ -262,6 +269,7 @@ FW_VERSIONS = {
|
|||
b'78109-TBC-A510\x00\x00',
|
||||
b'78109-TBC-A520\x00\x00',
|
||||
b'78109-TBC-A530\x00\x00',
|
||||
b'78109-TBC-C530\x00\x00',
|
||||
b'78109-TBH-A530\x00\x00',
|
||||
b'78109-TEG-A310\x00\x00',
|
||||
],
|
||||
|
@ -272,7 +280,6 @@ FW_VERSIONS = {
|
|||
b'36161-TEG-A010\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18daeff1, None): [
|
||||
b'36161-TBA-A030\x00\x00',
|
||||
b'38897-TBA-A010\x00\x00',
|
||||
b'38897-TBA-A020\x00\x00',
|
||||
],
|
||||
|
@ -285,11 +292,15 @@ FW_VERSIONS = {
|
|||
b'37805-5AN-A830\x00\x00',
|
||||
b'37805-5AN-A930\x00\x00',
|
||||
b'37805-5AN-L940\x00\x00',
|
||||
b'37805-5AN-LH20\x00\x00',
|
||||
b'37805-5AN-LJ20\x00\x00',
|
||||
b'37805-5AZ-E850\x00\x00',
|
||||
b'37805-5BB-L640\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da1ef1, None): [
|
||||
b'28101-5CG-A920\x00\x00',
|
||||
b'28101-5CG-C220\x00\x00',
|
||||
b'28101-5CG-C320\x00\x00',
|
||||
b'28101-5CK-A130\x00\x00',
|
||||
b'28101-5CK-A140\x00\x00',
|
||||
b'28101-5CK-A150\x00\x00',
|
||||
|
@ -301,6 +312,7 @@ FW_VERSIONS = {
|
|||
b'57114-TBG-A340\x00\x00',
|
||||
b'57114-TGG-A340\x00\x00',
|
||||
b'57114-TGL-G330\x00\x00',
|
||||
b'57114-TGG-C320\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TBA-C020\x00\x00',
|
||||
|
@ -308,31 +320,41 @@ FW_VERSIONS = {
|
|||
b'39990-TGG-A020\x00\x00',
|
||||
b'39990-TGG-A120\x00\x00',
|
||||
b'39990-TGL-E130\x00\x00',
|
||||
b'39990-TGG-A020\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da53f1, None): [
|
||||
b'77959-TBA-A060\x00\x00',
|
||||
b'77959-TGG-A020\x00\x00',
|
||||
b'77959-TGG-G010\x00\x00',
|
||||
b'77959-TGG-A020\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da60f1, None): [
|
||||
b'78109-TBA-A910\x00\x00',
|
||||
b'78109-TBC-A740\x00\x00',
|
||||
b'78109-TGG-A210\x00\x00',
|
||||
b'78109-TGG-A310\x00\x00',
|
||||
b'78109-TGG-A320\x00\x00',
|
||||
b'78109-TGG-A810\x00\x00',
|
||||
b'78109-TGG-A820\x00\x00',
|
||||
b'78109-TGL-G120\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18dab0f1, None): [
|
||||
b'36802-TBA-A150\x00\x00',
|
||||
b'36802-TGG-A050\x00\x00',
|
||||
b'36802-TGL-G040\x00\x00',
|
||||
b'36802-TGG-A060\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18dab5f1, None): [
|
||||
b'36161-TBA-A130\x00\x00',
|
||||
b'36161-TGG-A060\x00\x00',
|
||||
b'36161-TGL-G050\x00\x00',
|
||||
b'36161-TGG-A080\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18daeff1, None): [
|
||||
b'38897-TBA-A110\x00\x00',
|
||||
b'38897-TBA-A020\x00\x00',
|
||||
b'38897-TBA-A020\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18daeff1, None): [b'38897-TBA-A110\x00\x00', b'38897-TBA-A020\x00\x00'],
|
||||
},
|
||||
CAR.CRV_5G: {
|
||||
(Ecu.unknown, 0x18da10f1, None): [
|
||||
|
@ -344,6 +366,7 @@ FW_VERSIONS = {
|
|||
b'37805-5PA-A680\x00\x00',
|
||||
b'37805-5PA-A850\x00\x00',
|
||||
b'37805-5PA-A870\x00\x00',
|
||||
b'37805-5PA-A880\x00\x00',
|
||||
b'37805-5PA-A890\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da1ef1, None): [
|
||||
|
@ -422,6 +445,7 @@ FW_VERSIONS = {
|
|||
b'28102-5MX-A610\x00\x00',
|
||||
b'28102-5MX-A710\x00\x00',
|
||||
b'28102-5MX-A910\x00\x00',
|
||||
b'28102-5MX-C001\x00\x00',
|
||||
b'28103-5NZ-A300\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da28f1, None): [b'57114-THR-A040\x00\x00', b'57114-THR-A110\x00\x00'],
|
||||
|
@ -436,6 +460,7 @@ FW_VERSIONS = {
|
|||
b'78109-THR-AE40\x00\x00',
|
||||
b'78109-THR-AL10\x00\x00',
|
||||
b'78109-THR-C330\x00\x00',
|
||||
b'78109-THR-CE20\x00\x00',
|
||||
],
|
||||
(Ecu.unknown, 0x18da0bf1, None): [b'54008-THR-A020\x00\x00'],
|
||||
},
|
||||
|
|
|
@ -1,8 +1,8 @@
|
|||
from cereal import car
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
|
@ -124,27 +124,7 @@ def get_camera_parser(CP):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
self.left_blinker_on = 0
|
||||
self.left_blinker_flash = 0
|
||||
self.right_blinker_on = 0
|
||||
self.right_blinker_flash = 0
|
||||
class CarState(CarStateBase):
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# update prevs, update must run once per Loop
|
||||
|
@ -162,36 +142,26 @@ class CarState():
|
|||
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
self.pcm_acc_status = int(self.acc_active)
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
|
||||
self.low_speed_lockout = v_wheel < 1.0
|
||||
self.low_speed_lockout = self.v_ego_raw < 1.0
|
||||
|
||||
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
|
||||
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
|
||||
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
|
||||
self.standstill = not v_wheel > 0.1
|
||||
self.standstill = not self.v_ego_raw > 0.1
|
||||
|
||||
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
|
||||
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
|
||||
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
|
||||
self.main_on = True
|
||||
self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
|
||||
self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh']
|
||||
self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
|
||||
self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh']
|
||||
self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD
|
||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
|
||||
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
|
||||
|
|
|
@ -1,8 +1,12 @@
|
|||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
# generic car and radar interfaces
|
||||
|
||||
class CarInterfaceBase():
|
||||
|
@ -42,3 +46,31 @@ class RadarInterfaceBase():
|
|||
time.sleep(self.radar_ts) # radard runs on RI updates
|
||||
|
||||
return ret
|
||||
|
||||
class CarStateBase:
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
self.cruise_buttons = 0
|
||||
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, DT_CTRL], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
|
||||
def update_speed_kf(self, v_ego_raw):
|
||||
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_ego_raw], [0.0]]
|
||||
|
||||
v_ego_x = self.v_ego_kf.update(v_ego_raw)
|
||||
return float(v_ego_x[0]), float(v_ego_x[1])
|
||||
|
||||
@staticmethod
|
||||
def parse_gear_shifter(gear):
|
||||
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
|
||||
'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive,
|
||||
'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake}.get(gear, GearShifter.unknown)
|
||||
|
|
|
@ -53,7 +53,7 @@ class CarController():
|
|||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
lkas_enabled = enabled and not CS.steer_not_allowed
|
||||
lkas_enabled = enabled
|
||||
|
||||
if not lkas_enabled:
|
||||
apply_steer = 0
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
import copy
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
|
||||
|
||||
|
@ -82,29 +82,12 @@ def get_camera_can_parser(CP):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
# initialize can parser
|
||||
self.CP = CP
|
||||
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.left_blinker_on = False
|
||||
self.left_blinker_cnt = 0
|
||||
self.prev_left_blinker_on = False
|
||||
self.right_blinker_on = False
|
||||
self.right_blinker_cnt = 0
|
||||
self.prev_right_blinker_on = False
|
||||
self.steer_torque_driver = 0
|
||||
self.steer_not_allowed = False
|
||||
self.main_on = False
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
|
@ -124,16 +107,10 @@ class CarState():
|
|||
if cp.vl["Dash_State"]['Units'] == 1:
|
||||
self.v_cruise_pcm *= CV.MPH_TO_KPH
|
||||
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = self.v_ego_raw < 0.01
|
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
|
|
|
@ -23,12 +23,6 @@ ANGLE_DELTA_BP = [0., 5., 15.]
|
|||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
|
||||
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
|
||||
0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
|
||||
0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
|
||||
0x383]
|
||||
|
||||
|
||||
def accel_hysteresis(accel, accel_steady, enabled):
|
||||
|
||||
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
|
||||
|
|
|
@ -1,17 +1,10 @@
|
|||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def parse_gear_shifter(gear):
|
||||
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
|
||||
'D': GearShifter.drive, 'B': GearShifter.brake}.get(gear, GearShifter.unknown)
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
|
@ -89,30 +82,14 @@ def get_cam_can_parser(CP):
|
|||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState():
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
|
||||
self.left_blinker_on = 0
|
||||
self.right_blinker_on = 0
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
|
||||
self.angle_offset = 0.
|
||||
self.init_angle_offset = False
|
||||
|
||||
# initialize can parser
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
# R = 1e3
|
||||
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
|
||||
A=[[1.0, dt], [0.0, 1.0]],
|
||||
C=[1.0, 0.0],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
self.v_ego = 0.0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
|
@ -127,25 +104,16 @@ class CarState():
|
|||
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
else:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
self.car_gas = self.pedal_gas
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
self.v_ego_raw = v_wheel
|
||||
v_ego_x = self.v_ego_kf.update(v_wheel)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
self.standstill = not v_wheel > 0.001
|
||||
self.standstill = not self.v_ego_raw > 0.001
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR:
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
|
@ -161,7 +129,7 @@ class CarState():
|
|||
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
|
||||
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
|
||||
self.gear_shifter = parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
|
||||
else:
|
||||
|
|
|
@ -163,6 +163,16 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate == CAR.HIGHLANDER_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.84988 # 112.2 in = 2.84988 m
|
||||
ret.steerRatio = 16.0
|
||||
tire_stiffness_factor = 0.8
|
||||
ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.18], [0.015]] # community tuning
|
||||
ret.lateralTuning.pid.kf = 0.00012 # community tuning
|
||||
|
||||
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
|
@ -192,7 +202,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
|
||||
elif candidate == CAR.RAV4H_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
|
@ -253,6 +263,16 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007
|
||||
|
||||
elif candidate == CAR.LEXUS_NXH:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.66
|
||||
ret.steerRatio = 14.7
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
ret.steerRateCost = 1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
|
@ -276,11 +296,14 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.brakeMaxV = [1.]
|
||||
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay
|
||||
# Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it
|
||||
smartDsu = 0x2FF in fingerprint[0]
|
||||
# In TSS2 cars the camera does long control
|
||||
ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR
|
||||
ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.apgs)
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera and (ret.enableDsu or candidate in TSS2_CAR)
|
||||
# if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
|
||||
cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs)
|
||||
|
@ -291,7 +314,8 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
|
||||
|
||||
# removing the DSU disables AEB and it's considered a community maintained feature
|
||||
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu
|
||||
# intercepting the DSU is a community feature since it requires unofficial hardware
|
||||
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0., 9.]
|
||||
ret.longitudinalTuning.deadzoneV = [0., .15]
|
||||
|
@ -341,7 +365,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
ret.gas = self.CS.pedal_gas
|
||||
if self.CP.enableGasInterceptor:
|
||||
# use interceptor values to disengage on pedal press
|
||||
ret.gasPressed = self.CS.pedal_gas > 15
|
||||
|
|
|
@ -22,6 +22,7 @@ class CAR:
|
|||
CAMRY = "TOYOTA CAMRY 2018"
|
||||
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
|
||||
HIGHLANDER = "TOYOTA HIGHLANDER 2017"
|
||||
HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020"
|
||||
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
|
||||
AVALON = "TOYOTA AVALON 2016"
|
||||
RAV4_TSS2 = "TOYOTA RAV4 2019"
|
||||
|
@ -33,28 +34,29 @@ class CAR:
|
|||
LEXUS_IS = "LEXUS IS300 2018"
|
||||
LEXUS_CTH = "LEXUS CT 200H 2018"
|
||||
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
|
||||
LEXUS_NXH = "LEXUS NX300H 2018"
|
||||
|
||||
|
||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||
STATIC_MSGS = [
|
||||
(0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
|
||||
(0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
|
||||
(0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
|
||||
(0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
|
||||
(0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x2E6, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
|
||||
(0x2E7, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
|
||||
(0x33E, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
|
||||
(0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
(0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
(0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
|
||||
(0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'),
|
||||
(0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
|
||||
(0x292, Ecu.apgs, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'),
|
||||
(0x32E, Ecu.apgs, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
|
@ -67,195 +69,206 @@ STATIC_MSGS = [
|
|||
|
||||
ECU_FINGERPRINT = {
|
||||
Ecu.fwdCamera: [0x2e4], # steer torque cmd
|
||||
Ecu.dsu: [0x343], # accel cmd
|
||||
Ecu.dsu: [0x283], # accel cmd
|
||||
Ecu.apgs: [0x835], # angle cmd
|
||||
}
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.RAV4: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8
|
||||
}],
|
||||
CAR.RAV4H: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# Chinese RAV4
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
|
||||
}],
|
||||
CAR.PRIUS: [{
|
||||
# with ipas
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#2019 LE
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2020 Prius Prime LE
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767:4, 800: 8, 810: 2, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#2020 Prius Prime Limited
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
|
||||
},
|
||||
#2020 Central Europe Prime
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 8, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767:4, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 8, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8
|
||||
},
|
||||
#2017 German Prius
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296:8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8,740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767:4, 800: 8, 1863:8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996:8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8
|
||||
}],
|
||||
#Corolla w/ added Pedal Support (512L and 513L)
|
||||
CAR.COROLLA: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
|
||||
}],
|
||||
CAR.LEXUS_RX: [{
|
||||
# 2016 Lexus RX 350
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 812: 3, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
},
|
||||
# 2017 Lexus RX 350
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 658: 8, 705: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 658: 8, 705: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_RXH: [{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
|
||||
},
|
||||
# RX450HL
|
||||
# TODO: get proper fingerprint in stock mode
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# RX540H 2019 with color hud
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2017 RX 450h
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 744: 8, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 744: 8, 767:4, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.LEXUS_RX_TSS2: [
|
||||
# 2020 Lexus RX 350
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594:8, 1595: 8, 1600: 8, 1649: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594:8, 1595: 8, 1600: 8, 1649: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.CHR: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 913: 8, 918: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 705: 8, 740: 5, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 913: 8, 918: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8
|
||||
}],
|
||||
CAR.CHRH: [{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 800: 8, 810: 2, 812: 8, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.CAMRY: [
|
||||
#XLE and LE
|
||||
{
|
||||
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#XSE and SE
|
||||
# TODO: get proper fingerprint in stock mode
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
{
|
||||
# 2019 XSE
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1800: 8, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1767:4, 800: 8, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.CAMRYH: [
|
||||
#SE, LE and LE with Blindspot Monitor
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#SL
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#XLE
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.HIGHLANDER: [{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2019 Highlander XLE
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2017 Highlander Limited
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2018 Highlander Limited Platinum
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1988: 8, 1990: 8, 1996: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1988: 8, 1990: 8, 1996: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.HIGHLANDER_TSS2: [{
|
||||
# 2020 highlander limited
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.HIGHLANDERH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
{
|
||||
# 2019 Highlander Hybrid Limited Platinum
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.AVALON: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.RAV4_TSS2: [
|
||||
# LE
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# XLE, Limited, and AWD
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.COROLLA_TSS2: [
|
||||
# hatch 2019+ and sedan 2020+
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8
|
||||
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8
|
||||
}],
|
||||
CAR.COROLLAH_TSS2: [
|
||||
# 2019 Taiwan Altis Hybrid
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 918: 7, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1082: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 767:4, 800: 8, 810: 2, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 918: 7, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1082: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
},
|
||||
# 2019 Chinese Levin Hybrid
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
}
|
||||
],
|
||||
CAR.LEXUS_ES_TSS2: [{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8,
|
||||
}],
|
||||
CAR.LEXUS_ESH_TSS2: [
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.SIENNA: [
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 767:4, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# XLE AWD 2018
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 767:4, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_IS: [
|
||||
# IS300 2018
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# IS300H 2017
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767:4, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.RAV4H_TSS2: [
|
||||
#Hybrid Limited
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913:8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913:8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# German Lounge
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1800: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
}],
|
||||
],
|
||||
CAR.LEXUS_CTH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1116: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_NXH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 822: 8, 824: 8, 835: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 889: 8, 891: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 987: 8, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}]
|
||||
}
|
||||
|
||||
# Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection
|
||||
IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2]
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.AVALON: {
|
||||
(Ecu.esp, 0x7b0, None): [b'F152607060\x00\x00\x00\x00\x00\x00'],
|
||||
|
@ -267,6 +280,7 @@ FW_VERSIONS = {
|
|||
},
|
||||
CAR.CAMRY: {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x018966306L5200\x00\x00\x00\x00',
|
||||
b'\x018966333P4200\x00\x00\x00\x00',
|
||||
b'\x018966333P4300\x00\x00\x00\x00',
|
||||
b'\x018966333P4400\x00\x00\x00\x00',
|
||||
|
@ -325,9 +339,18 @@ FW_VERSIONS = {
|
|||
b'8646F0605000 ',
|
||||
],
|
||||
},
|
||||
CAR.CHR: {
|
||||
(Ecu.dsu, 0x791, None): [b'8821FF404100 '],
|
||||
(Ecu.esp, 0x7b0, None): [b'F1526F4122\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.eps, 0x7a1, None): [b'8965B10040\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.engine, 0x7e0, None): [b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [b'8821FF404100 '],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646FF404000 '],
|
||||
},
|
||||
CAR.COROLLA: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x01896630E88000\x00\x00\x00\x00',
|
||||
b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0230ZC2100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0230ZC2200\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0230ZC2300\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
|
@ -494,9 +517,11 @@ FW_VERSIONS = {
|
|||
b'\x02896634774100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
|
||||
b'\x02896634774200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
|
||||
b'\x02896634782000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
|
||||
b'\x02896634784000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
|
||||
b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00',
|
||||
b'\x03896634759300\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00',
|
||||
b'\x03896634760000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701002\x00\x00\x00\x00',
|
||||
b'\x03896634760100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00',
|
||||
b'\x03896634760200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00',
|
||||
b'\x03896634760200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701004\x00\x00\x00\x00',
|
||||
b'\x03896634768000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703001\x00\x00\x00\x00',
|
||||
|
@ -551,6 +576,7 @@ FW_VERSIONS = {
|
|||
},
|
||||
CAR.RAV4: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x02342Q1000\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02342Q1100\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02342Q1300\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02342Q2000\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
|
@ -561,6 +587,7 @@ FW_VERSIONS = {
|
|||
b'8965B42083\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'F15260R102\x00\x00\x00\x00\x00\x00',
|
||||
b'F15260R103\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642493\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
|
@ -619,9 +646,11 @@ FW_VERSIONS = {
|
|||
b'\x018966342V3100\x00\x00\x00\x00',
|
||||
b'\x018966342X5000\x00\x00\x00\x00',
|
||||
b'\x01896634A05000\x00\x00\x00\x00',
|
||||
b'\x01896634A19000\x00\x00\x00\x00',
|
||||
b'\x01896634A22000\x00\x00\x00\x00',
|
||||
b'\x01F152642551\x00\x00\x00\x00\x00\x00',
|
||||
b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'F152606230\x00\x00\x00\x00\x00\x00',
|
||||
|
@ -695,9 +724,17 @@ FW_VERSIONS = {
|
|||
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F5301400\x00\x00\x00\x00'],
|
||||
},
|
||||
CAR.SIENNA: {
|
||||
(Ecu.engine, 0x700, None): [b'\x01896630832100\x00\x00\x00\x00'],
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x01896630832100\x00\x00\x00\x00',
|
||||
b'\x01896630842000\x00\x00\x00\x00',
|
||||
b'\x01896630851100\x00\x00\x00\x00',
|
||||
b'\x01896630860000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [b'8965B45070\x00\x00\x00\x00\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'8821F4702100\x00\x00\x00\x00',
|
||||
b'8821F4702300\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F0801100\x00\x00\x00\x00'],
|
||||
},
|
||||
CAR.LEXUS_RXH: {
|
||||
|
@ -747,6 +784,7 @@ DBC = {
|
|||
CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
|
||||
CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
|
@ -758,8 +796,9 @@ DBC = {
|
|||
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'),
|
||||
}
|
||||
|
||||
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2]
|
||||
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2]
|
||||
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2] # no resume button press required
|
||||
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2]
|
||||
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2]
|
||||
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2] # no resume button press required
|
||||
|
|
|
@ -1,13 +1,10 @@
|
|||
import numpy as np
|
||||
from cereal import car
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams
|
||||
|
||||
GEAR = car.CarState.GearShifter
|
||||
|
||||
def get_mqb_pt_can_parser(CP, canbus):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
signals = [
|
||||
|
@ -99,29 +96,14 @@ def get_mqb_cam_can_parser(CP, canbus):
|
|||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam)
|
||||
|
||||
def parse_gear_shifter(gear):
|
||||
# Return mapping of gearshift position to selected gear.
|
||||
return {'P': GEAR.park, 'R': GEAR.reverse, 'N': GEAR.neutral,
|
||||
'D': GEAR.drive, 'E': GEAR.eco, 'S': GEAR.sport, 'T': GEAR.manumatic}.get(gear, GEAR.unknown)
|
||||
|
||||
class CarState():
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, canbus):
|
||||
# initialize can parser
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe']
|
||||
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe']
|
||||
self.buttonStates = BUTTON_STATES.copy()
|
||||
|
||||
# vEgo Kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
|
||||
def update(self, pt_cp):
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
|
@ -130,9 +112,8 @@ class CarState():
|
|||
self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
|
||||
self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR]))
|
||||
v_ego_x = self.v_ego_kf.update(self.vEgoRaw)
|
||||
self.vEgo = float(v_ego_x[0])
|
||||
self.aEgo = float(v_ego_x[1])
|
||||
self.vEgo, self.aEgo = self.update_speed_kf(self.vEgoRaw)
|
||||
|
||||
self.standstill = self.vEgoRaw < 0.1
|
||||
|
||||
# Update steering angle, rate, yaw rate, and driver input torque. VW send
|
||||
|
@ -152,7 +133,7 @@ class CarState():
|
|||
|
||||
# Update gear and/or clutch position data.
|
||||
can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'])
|
||||
self.gearShifter = parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
self.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
|
||||
|
||||
# Update door and trunk/hatch lid open status.
|
||||
self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],
|
||||
|
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.7.2-release"
|
||||
#define COMMA_VERSION "0.7.3-release"
|
||||
|
|
|
@ -6,6 +6,7 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
|||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
|
||||
from selfdrive.controls.lib.lane_planner import LanePlanner
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.params import Params
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
|
||||
|
@ -53,6 +54,7 @@ class PathPlanner():
|
|||
|
||||
self.setup_mpc()
|
||||
self.solution_invalid_cnt = 0
|
||||
self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1'
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
|
@ -97,7 +99,7 @@ class PathPlanner():
|
|||
elif sm['carState'].rightBlinker:
|
||||
self.lane_change_direction = LaneChangeDirection.right
|
||||
|
||||
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker):
|
||||
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled):
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
else:
|
||||
|
|
|
@ -1,64 +1,72 @@
|
|||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
|
||||
import numpy as np
|
||||
|
||||
from cereal.messaging import SubMaster
|
||||
|
||||
|
||||
def cputime_total(ct):
|
||||
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
|
||||
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
|
||||
|
||||
|
||||
def cputime_busy(ct):
|
||||
return ct.user + ct.nice + ct.system + ct.irq + ct.softirq
|
||||
return ct.user + ct.nice + ct.system + ct.irq + ct.softirq
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--mem', action='store_true')
|
||||
args = parser.parse_args()
|
||||
|
||||
sm = SubMaster(['thermal', 'procLog'])
|
||||
sm = SubMaster(['thermal', 'procLog'])
|
||||
|
||||
last_temp = 0.0
|
||||
last_mem = 0.0
|
||||
total_times = [0., 0., 0., 0.]
|
||||
busy_times = [0., 0., 0.0, 0.]
|
||||
last_temp = 0.0
|
||||
last_mem = 0.0
|
||||
total_times = [0., 0., 0., 0.]
|
||||
busy_times = [0., 0., 0.0, 0.]
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
if sm.updated['thermal']:
|
||||
t = sm['thermal']
|
||||
last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10.
|
||||
last_mem = t.memUsedPercent
|
||||
|
||||
if sm.updated['procLog']:
|
||||
m = sm['procLog']
|
||||
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
cores = [0., 0., 0., 0.]
|
||||
total_times_new = [0., 0., 0., 0.]
|
||||
busy_times_new = [0., 0., 0.0, 0.]
|
||||
|
||||
if sm.updated['thermal']:
|
||||
t = sm['thermal']
|
||||
last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10.
|
||||
last_mem = t.memUsedPercent
|
||||
for c in m.cpuTimes:
|
||||
n = c.cpuNum
|
||||
total_times_new[n] = cputime_total(c)
|
||||
busy_times_new[n] = cputime_busy(c)
|
||||
|
||||
if sm.updated['procLog']:
|
||||
m = sm['procLog']
|
||||
for n in range(4):
|
||||
t_busy = busy_times_new[n] - busy_times[n]
|
||||
t_total = total_times_new[n] - total_times[n]
|
||||
cores[n] = t_busy / t_total
|
||||
|
||||
mems = {}
|
||||
for proc in m.procs:
|
||||
name = proc.name
|
||||
if len(proc.cmdline):
|
||||
name = proc.cmdline[0]
|
||||
if len(proc.exe):
|
||||
name = proc.exe + " - " + name
|
||||
mems[name] = float(proc.memRss) / 1e6
|
||||
total_times = total_times_new[:]
|
||||
busy_times = busy_times_new[:]
|
||||
|
||||
cores = [0., 0., 0., 0.]
|
||||
total_times_new = [0., 0., 0., 0.]
|
||||
busy_times_new = [0., 0., 0.0, 0.]
|
||||
print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * np.mean(cores), last_mem, last_temp))
|
||||
|
||||
for c in m.cpuTimes:
|
||||
n = c.cpuNum
|
||||
total_times_new[n] = cputime_total(c)
|
||||
busy_times_new[n] = cputime_busy(c)
|
||||
|
||||
for n in range(4):
|
||||
t_busy = busy_times_new[n] - busy_times[n]
|
||||
t_total = total_times_new[n] - total_times[n]
|
||||
cores[n] = t_busy / t_total
|
||||
|
||||
total_times = total_times_new[:]
|
||||
busy_times = busy_times_new[:]
|
||||
|
||||
print()
|
||||
print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * np.mean(cores), last_mem, last_temp))
|
||||
print("Top memory usage:")
|
||||
for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]:
|
||||
print(f"{k.rjust(70)} {v:.2f} MB")
|
||||
if args.mem:
|
||||
mems = {}
|
||||
for proc in m.procs:
|
||||
name = proc.name
|
||||
if len(proc.cmdline):
|
||||
name = proc.cmdline[0]
|
||||
if len(proc.exe):
|
||||
name = proc.exe + " - " + name
|
||||
mems[name] = float(proc.memRss) / 1e6
|
||||
print("Top memory usage:")
|
||||
for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]:
|
||||
print(f"{k.rjust(70)} {v:.2f} MB")
|
||||
print()
|
||||
|
|
|
@ -1,4 +1,5 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import traceback
|
||||
import sys
|
||||
from tqdm import tqdm
|
||||
|
@ -13,14 +14,19 @@ from selfdrive.car.honda.values import FINGERPRINTS as HONDA_FINGERPRINTS
|
|||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) < 2:
|
||||
print("Usage: ./test_fw_query_on_routes.py <route_list>")
|
||||
print("Usage: ./test_fw_query_on_routes.py <route_list>/<route>")
|
||||
sys.exit(1)
|
||||
|
||||
if os.path.exists(sys.argv[1]):
|
||||
routes = list(open(sys.argv[1]))
|
||||
else:
|
||||
routes = [sys.argv[1]]
|
||||
|
||||
wrong = 0
|
||||
good = 0
|
||||
|
||||
dongles = []
|
||||
for route in tqdm(list(open(sys.argv[1]))):
|
||||
for route in tqdm(routes):
|
||||
route = route.rstrip()
|
||||
dongle_id, time = route.split('|')
|
||||
qlog_path = f"cd:/{dongle_id}/{time}/0/qlog.bz2"
|
||||
|
|
|
@ -0,0 +1 @@
|
|||
generated/
|
|
@ -0,0 +1,52 @@
|
|||
# Kalman filter library
|
||||
|
||||
## Introduction
|
||||
The kalman filter framework described here is an incredibly powerful tool for any optimization problem,
|
||||
but particularly for visual odometry, sensor fusion localization or SLAM. It is designed to provide very
|
||||
accurate results, work online or offline, be fairly computationally efficient, be easy to design filters with in
|
||||
python.
|
||||
|
||||
## Feature walkthrough
|
||||
|
||||
### Extended Kalman Filter with symbolic Jacobian computation
|
||||
Most dynamic systems can be described as a Hidden Markov Process. To estimate the state of such a system with noisy
|
||||
measurements one can use a Recursive Bayesian estimator. For a linear Markov Process a regular linear Kalman filter is optimal.
|
||||
Unfortunately, a lot of systems are non-linear. Extended Kalman Filters can model systems by linearizing the non-linear
|
||||
system at every step, this provides a close to optimal estimator when the linearization is good enough. If the linearization
|
||||
introduces too much noise, one can use an Iterated Extended Kalman Filter, Unscented Kalman Filter or a Particle Filter. For
|
||||
most applications those estimators are overkill and introduce too much complexity and require a lot of additional compute.
|
||||
|
||||
Conventionally Extended Kalman Filters are implemented by writing the system's dynamic equations and then manually symbolically
|
||||
calculating the Jacobians for the linearization. For complex systems this is time consuming and very prone to calculation errors.
|
||||
This library symbolically computes the Jacobians using sympy to simplify the system's definition and remove the possiblity of introducing calculation errors.
|
||||
|
||||
### Error State Kalman Filter
|
||||
3D localization algorithms ussually also require estimating orientation of an object in 3D. Orientation is generally represented
|
||||
with euler angles or quaternions.
|
||||
|
||||
Euler angles have several problems, there are mulitple ways to represent the same orientation,
|
||||
gimbal lock can cause the loss of a degree of freedom and lastly their behaviour is very non-linear when errors are large.
|
||||
Quaternions with one strictly positive dimension don't suffer from these issues, but have another set of problems.
|
||||
Quaternions need to be normalized otherwise they will grow unbounded, this is cannot be cleanly enforced in a kalman filter.
|
||||
Most importantly though a quaternion has 4 dimensions, but only represents 3 degrees of freedom, so there is one redundant dimension.
|
||||
|
||||
Kalman filters are designed to minimize the error of the system's state. It is possible to have a kalman filter where state and the error of the state are represented in a different space. As long as there is an error function that can compute the error based on the true state and estimated state. It is problematic to have redundant dimensions in the error of the kalman filter, but not in the state. A good compromise then, is to use the quaternion to represent the system's attitude state and use euler angles to describe the error in attitude. This library supports and defining an arbitrary error that is in a different space than the state. [Joan Solà](https://arxiv.org/abs/1711.02508) has written a comprehensive description of using ESKFs for robust 3D orientation estimation.
|
||||
|
||||
### Multi-State Constraint Kalman Filter
|
||||
How do you integrate feature-based visual odometry with a Kalman filter? The problem is that one cannot write an observation equation for 2D feature observations in image space for a localization kalman filter. One needs to give the feature observation a depth so it has a 3D position, then one can write an obvervation equation in the kalman filter. This is possible by tracking the feature across frames and then estimating the depth. However, the solution is not that simple, the depth estimated by tracking the feature across frames depends on the location of the camera at those frames, and thus the state of the kalman filter. This creates a positive feedback loop where the kalman filter wrongly gains confidence in it's position because the feature position updates reinforce it.
|
||||
|
||||
The solution is to use an [MSCKF](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf), which this library fully supports.
|
||||
|
||||
### Rauch–Tung–Striebel smoothing
|
||||
When doing offline estimation with a kalman filter there can be an initialization period where states are badly estimated.
|
||||
Global estimators don't suffer from this, to make our kalman filter competitive with global optimizers we can run the filter
|
||||
backwards using an RTS smoother. Those combined with potentially multiple forward and backwards passes of the data should make
|
||||
performance very close to global optimization.
|
||||
|
||||
### Mahalanobis distance outlier rejector
|
||||
A lot of measurements do not come from a Gaussian distribution and as such have outliers that do not fit the statistical model
|
||||
of the Kalman filter. This can cause a lot of performance issues if not dealt with. This library allows the use of a mahalanobis
|
||||
distance statistical test on the incoming measurements to deal with this. Note that good initialization is critical to prevent
|
||||
good measurements from being rejected.
|
||||
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
Import('env')
|
||||
|
||||
templates = Glob('templates/*')
|
||||
sympy_helpers = "helpers/sympy_helpers.py"
|
||||
ekf_sym = "helpers/ekf_sym.py"
|
||||
|
||||
to_build = {
|
||||
'pos_computer_4': 'helpers/lst_sq_computer.py',
|
||||
'pos_computer_5': 'helpers/lst_sq_computer.py',
|
||||
'feature_handler_5': 'helpers/feature_handler.py',
|
||||
'gnss': 'models/gnss_kf.py',
|
||||
'loc_4': 'models/loc_kf.py',
|
||||
'live': 'models/live_kf.py',
|
||||
'lane': '#xx/pipeline/lib/ekf/lane_kf.py',
|
||||
}
|
||||
|
||||
found = {}
|
||||
|
||||
for target, command in to_build.items():
|
||||
if File(command).exists():
|
||||
found[target] = command
|
||||
|
||||
for target, command in found.items():
|
||||
target_files = File([f'generated/{target}.cpp', f'generated/{target}.h'])
|
||||
command_file = File(command)
|
||||
env.Command(target_files,
|
||||
[templates, command_file, sympy_helpers, ekf_sym],
|
||||
command_file.get_abspath()+" "+target
|
||||
)
|
||||
|
||||
env.SharedLibrary('generated/' + target, target_files[0])
|
|
@ -0,0 +1,191 @@
|
|||
import numpy as np
|
||||
import os
|
||||
from bisect import bisect
|
||||
from tqdm import tqdm
|
||||
from cffi import FFI
|
||||
|
||||
TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates'))
|
||||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'generated'))
|
||||
|
||||
|
||||
def write_code(name, code, header):
|
||||
if not os.path.exists(GENERATED_DIR):
|
||||
os.mkdir(GENERATED_DIR)
|
||||
|
||||
open(os.path.join(GENERATED_DIR, f"{name}.cpp"), 'w').write(code)
|
||||
open(os.path.join(GENERATED_DIR, f"{name}.h"), 'w').write(header)
|
||||
|
||||
|
||||
def load_code(name):
|
||||
shared_fn = os.path.join(GENERATED_DIR, f"lib{name}.so")
|
||||
header_fn = os.path.join(GENERATED_DIR, f"{name}.h")
|
||||
header = open(header_fn).read()
|
||||
|
||||
ffi = FFI()
|
||||
ffi.cdef(header)
|
||||
return (ffi, ffi.dlopen(shared_fn))
|
||||
|
||||
|
||||
class KalmanError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class ObservationKind():
|
||||
UNKNOWN = 0
|
||||
NO_OBSERVATION = 1
|
||||
GPS_NED = 2
|
||||
ODOMETRIC_SPEED = 3
|
||||
PHONE_GYRO = 4
|
||||
GPS_VEL = 5
|
||||
PSEUDORANGE_GPS = 6
|
||||
PSEUDORANGE_RATE_GPS = 7
|
||||
SPEED = 8
|
||||
NO_ROT = 9
|
||||
PHONE_ACCEL = 10
|
||||
ORB_POINT = 11
|
||||
ECEF_POS = 12
|
||||
CAMERA_ODO_TRANSLATION = 13
|
||||
CAMERA_ODO_ROTATION = 14
|
||||
ORB_FEATURES = 15
|
||||
MSCKF_TEST = 16
|
||||
FEATURE_TRACK_TEST = 17
|
||||
LANE_PT = 18
|
||||
IMU_FRAME = 19
|
||||
PSEUDORANGE_GLONASS = 20
|
||||
PSEUDORANGE_RATE_GLONASS = 21
|
||||
PSEUDORANGE = 22
|
||||
PSEUDORANGE_RATE = 23
|
||||
|
||||
names = ['Unknown',
|
||||
'No observation',
|
||||
'GPS NED',
|
||||
'Odometric speed',
|
||||
'Phone gyro',
|
||||
'GPS velocity',
|
||||
'GPS pseudorange',
|
||||
'GPS pseudorange rate',
|
||||
'Speed',
|
||||
'No rotation',
|
||||
'Phone acceleration',
|
||||
'ORB point',
|
||||
'ECEF pos',
|
||||
'camera odometric translation',
|
||||
'camera odometric rotation',
|
||||
'ORB features',
|
||||
'MSCKF test',
|
||||
'Feature track test',
|
||||
'Lane ecef point',
|
||||
'imu frame eulers',
|
||||
'GLONASS pseudorange',
|
||||
'GLONASS pseudorange rate']
|
||||
|
||||
@classmethod
|
||||
def to_string(cls, kind):
|
||||
return cls.names[kind]
|
||||
|
||||
|
||||
|
||||
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GPS,
|
||||
ObservationKind.PSEUDORANGE_GLONASS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GLONASS]
|
||||
|
||||
|
||||
def run_car_ekf_offline(kf, observations_by_kind):
|
||||
from laika.raw_gnss import GNSSMeasurement
|
||||
observations = []
|
||||
# create list of observations with element format: [kind, time, data]
|
||||
for kind in observations_by_kind:
|
||||
for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]):
|
||||
observations.append([t, kind, data])
|
||||
observations.sort(key=lambda obs: obs[0])
|
||||
|
||||
times, estimates = run_observations_through_filter(kf, observations)
|
||||
|
||||
forward_states = np.stack(e[1] for e in estimates)
|
||||
forward_covs = np.stack(e[3] for e in estimates)
|
||||
smoothed_states, smoothed_covs = kf.rts_smooth(estimates)
|
||||
|
||||
observations_dict = {}
|
||||
# TODO assuming observations and estimates
|
||||
# are same length may not work with VO
|
||||
for e in estimates:
|
||||
t = e[4]
|
||||
kind = str(int(e[5]))
|
||||
res = e[6]
|
||||
z = e[7]
|
||||
ea = e[8]
|
||||
if len(z) == 0:
|
||||
continue
|
||||
if kind not in observations_dict:
|
||||
observations_dict[kind] = {}
|
||||
observations_dict[kind]['t'] = np.array(len(z)*[t])
|
||||
observations_dict[kind]['z'] = np.array(z)
|
||||
observations_dict[kind]['ea'] = np.array(ea)
|
||||
observations_dict[kind]['residual'] = np.array(res)
|
||||
else:
|
||||
observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t]))
|
||||
observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z)))
|
||||
observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea)))
|
||||
observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res)))
|
||||
|
||||
# add svIds to gnss data
|
||||
for kind in map(str, SAT_OBS):
|
||||
if int(kind) in observations_by_kind and kind in observations_dict:
|
||||
observations_dict[kind]['svIds'] = np.array([])
|
||||
observations_dict[kind]['CNO'] = np.array([])
|
||||
observations_dict[kind]['std'] = np.array([])
|
||||
for obs in observations_by_kind[int(kind)][1]:
|
||||
observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'],
|
||||
np.array([obs[:,GNSSMeasurement.PRN]]))
|
||||
observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'],
|
||||
np.array([obs[:,GNSSMeasurement.PR_STD]]))
|
||||
return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict
|
||||
|
||||
|
||||
def run_observations_through_filter(kf, observations, filter_time=None):
|
||||
estimates = []
|
||||
|
||||
for obs in tqdm(observations):
|
||||
t = obs[0]
|
||||
kind = obs[1]
|
||||
data = obs[2]
|
||||
estimates.append(kf.predict_and_observe(t, kind, data))
|
||||
times = [x[4] for x in estimates]
|
||||
return times, estimates
|
||||
|
||||
|
||||
def save_residuals_plot(obs, save_path, data_name):
|
||||
import matplotlib.pyplot as plt
|
||||
import mpld3 # pylint: disable=import-error
|
||||
fig = plt.figure(figsize=(10,20))
|
||||
fig.suptitle('Residuals of ' + data_name, fontsize=24)
|
||||
n = len(list(obs.keys()))
|
||||
start_times = [obs[kind]['t'][0] for kind in obs]
|
||||
start_time = min(start_times)
|
||||
xlims = [start_time + 3, start_time + 60]
|
||||
|
||||
for i, kind in enumerate(obs):
|
||||
ax = fig.add_subplot(n, 1, i+1)
|
||||
ax.set_xlim(xlims)
|
||||
t = obs[kind]['t']
|
||||
res = obs[kind]['residual']
|
||||
start_idx = bisect(t, xlims[0])
|
||||
if len(res) == start_idx:
|
||||
continue
|
||||
ylim = max(np.linalg.norm(res[start_idx:], axis=1))
|
||||
ax.set_ylim([-ylim, ylim])
|
||||
if int(kind) in SAT_OBS:
|
||||
svIds = obs[kind]['svIds']
|
||||
for svId in set(svIds):
|
||||
svId_idx = (svIds == svId)
|
||||
t = obs[kind]['t'][svId_idx]
|
||||
res = obs[kind]['residual'][svId_idx]
|
||||
ax.plot(t, res, label='SV ' + str(int(svId)))
|
||||
ax.legend(loc='right')
|
||||
else:
|
||||
ax.plot(t, res)
|
||||
plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20)
|
||||
plt.tight_layout()
|
||||
os.makedirs(save_path)
|
||||
mpld3.save_html(fig, save_path + 'residuals_plot.html')
|
|
@ -0,0 +1,22 @@
|
|||
import os
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def gen_chi2_ppf_lookup(max_dim=200):
|
||||
from scipy.stats import chi2
|
||||
table = np.zeros((max_dim, 98))
|
||||
for dim in range(1, max_dim):
|
||||
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
|
||||
|
||||
np.save('chi2_lookup_table', table)
|
||||
|
||||
|
||||
def chi2_ppf(p, dim):
|
||||
table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
|
||||
result = np.interp(p, np.arange(.01, .99, .01), table[dim])
|
||||
return result
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gen_chi2_ppf_lookup()
|
Binary file not shown.
|
@ -0,0 +1,563 @@
|
|||
import os
|
||||
from bisect import bisect_right
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
from numpy import dot
|
||||
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import sympy_into_c
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
|
||||
from selfdrive.locationd.kalman.helpers.chi2_lookup import chi2_ppf
|
||||
|
||||
|
||||
def solve(a, b):
|
||||
if a.shape[0] == 1 and a.shape[1] == 1:
|
||||
return b / a[0][0]
|
||||
else:
|
||||
return np.linalg.solve(a, b)
|
||||
|
||||
|
||||
def null(H, eps=1e-12):
|
||||
u, s, vh = np.linalg.svd(H)
|
||||
padding = max(0, np.shape(H)[1] - np.shape(s)[0])
|
||||
null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0)
|
||||
null_space = np.compress(null_mask, vh, axis=0)
|
||||
return np.transpose(null_space)
|
||||
|
||||
|
||||
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
|
||||
# optional state transition matrix, H modifier
|
||||
# and err_function if an error-state kalman filter (ESKF)
|
||||
# is desired. Best described in "Quaternion kinematics
|
||||
# for the error-state Kalman filter" by Joan Sola
|
||||
|
||||
if eskf_params:
|
||||
err_eqs = eskf_params[0]
|
||||
inv_err_eqs = eskf_params[1]
|
||||
H_mod_sym = eskf_params[2]
|
||||
f_err_sym = eskf_params[3]
|
||||
x_err_sym = eskf_params[4]
|
||||
else:
|
||||
nom_x = sp.MatrixSymbol('nom_x', dim_x, 1)
|
||||
true_x = sp.MatrixSymbol('true_x', dim_x, 1)
|
||||
delta_x = sp.MatrixSymbol('delta_x', dim_x, 1)
|
||||
err_function_sym = sp.Matrix(nom_x + delta_x)
|
||||
inv_err_function_sym = sp.Matrix(true_x - nom_x)
|
||||
err_eqs = [err_function_sym, nom_x, delta_x]
|
||||
inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
|
||||
|
||||
H_mod_sym = sp.Matrix(np.eye(dim_x))
|
||||
f_err_sym = f_sym
|
||||
x_err_sym = x_sym
|
||||
|
||||
# This configures the multi-state augmentation
|
||||
# needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
|
||||
if msckf_params:
|
||||
msckf = True
|
||||
dim_main = msckf_params[0] # size of the main state
|
||||
dim_augment = msckf_params[1] # size of one augment state chunk
|
||||
dim_main_err = msckf_params[2]
|
||||
dim_augment_err = msckf_params[3]
|
||||
N = msckf_params[4]
|
||||
feature_track_kinds = msckf_params[5]
|
||||
assert dim_main + dim_augment * N == dim_x
|
||||
assert dim_main_err + dim_augment_err * N == dim_err
|
||||
else:
|
||||
msckf = False
|
||||
dim_main = dim_x
|
||||
dim_augment = 0
|
||||
dim_main_err = dim_err
|
||||
dim_augment_err = 0
|
||||
N = 0
|
||||
|
||||
# linearize with jacobians
|
||||
F_sym = f_err_sym.jacobian(x_err_sym)
|
||||
for sym in x_err_sym:
|
||||
F_sym = F_sym.subs(sym, 0)
|
||||
for i in range(len(obs_eqs)):
|
||||
obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
|
||||
if msckf and obs_eqs[i][1] in feature_track_kinds:
|
||||
obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
|
||||
else:
|
||||
obs_eqs[i].append(None)
|
||||
|
||||
# collect sympy functions
|
||||
sympy_functions = []
|
||||
|
||||
# error functions
|
||||
sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
|
||||
sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
|
||||
|
||||
# H modifier for ESKF updates
|
||||
sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym]))
|
||||
|
||||
# state propagation function
|
||||
sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym]))
|
||||
sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym]))
|
||||
|
||||
# observation functions
|
||||
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
|
||||
sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym]))
|
||||
sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym]))
|
||||
if msckf and kind in feature_track_kinds:
|
||||
sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
|
||||
|
||||
# Generate and wrap all th c code
|
||||
header, code = sympy_into_c(sympy_functions)
|
||||
extra_header = "#define DIM %d\n" % dim_x
|
||||
extra_header += "#define EDIM %d\n" % dim_err
|
||||
extra_header += "#define MEDIM %d\n" % dim_main_err
|
||||
extra_header += "typedef void (*Hfun)(double *, double *, double *);\n"
|
||||
|
||||
extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);"
|
||||
|
||||
extra_post = ""
|
||||
|
||||
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
|
||||
if msckf and kind in feature_track_kinds:
|
||||
He_str = 'He_%d' % kind
|
||||
# ea_dim = ea_sym.shape[0]
|
||||
else:
|
||||
He_str = 'NULL'
|
||||
# ea_dim = 1 # not really dim of ea but makes c function work
|
||||
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
|
||||
maha_test = kind in maha_test_kinds
|
||||
extra_post += """
|
||||
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d);
|
||||
}
|
||||
""" % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind)
|
||||
extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh)
|
||||
extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind
|
||||
|
||||
code += '\nextern "C"{\n' + extra_header + "\n}\n"
|
||||
code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read()
|
||||
code += '\nextern "C"{\n' + extra_post + "\n}\n"
|
||||
header += "\n" + extra_header
|
||||
|
||||
write_code(name, code, header)
|
||||
|
||||
|
||||
class EKF_sym():
|
||||
def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
|
||||
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
|
||||
"""Generates process function and all observation functions for the kalman filter."""
|
||||
self.msckf = N > 0
|
||||
self.N = N
|
||||
self.dim_augment = dim_augment
|
||||
self.dim_augment_err = dim_augment_err
|
||||
self.dim_main = dim_main
|
||||
self.dim_main_err = dim_main_err
|
||||
|
||||
# state
|
||||
x_initial = x_initial.reshape((-1, 1))
|
||||
self.dim_x = x_initial.shape[0]
|
||||
self.dim_err = P_initial.shape[0]
|
||||
assert dim_main + dim_augment * N == self.dim_x
|
||||
assert dim_main_err + dim_augment_err * N == self.dim_err
|
||||
assert Q.shape == P_initial.shape
|
||||
|
||||
# kinds that should get mahalanobis distance
|
||||
# tested for outlier rejection
|
||||
self.maha_test_kinds = maha_test_kinds
|
||||
|
||||
# process noise
|
||||
self.Q = Q
|
||||
|
||||
# rewind stuff
|
||||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
self.rewind_obscache = []
|
||||
self.init_state(x_initial, P_initial, None)
|
||||
|
||||
ffi, lib = load_code(name)
|
||||
kinds, self.feature_track_kinds = [], []
|
||||
for func in dir(lib):
|
||||
if func[:2] == 'h_':
|
||||
kinds.append(int(func[2:]))
|
||||
if func[:3] == 'He_':
|
||||
self.feature_track_kinds.append(int(func[3:]))
|
||||
|
||||
# wrap all the sympy functions
|
||||
def wrap_1lists(name):
|
||||
func = eval("lib.%s" % name, {"lib": lib})
|
||||
|
||||
def ret(lst1, out):
|
||||
func(ffi.cast("double *", lst1.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return ret
|
||||
|
||||
def wrap_2lists(name):
|
||||
func = eval("lib.%s" % name, {"lib": lib})
|
||||
|
||||
def ret(lst1, lst2, out):
|
||||
func(ffi.cast("double *", lst1.ctypes.data),
|
||||
ffi.cast("double *", lst2.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return ret
|
||||
|
||||
def wrap_1list_1float(name):
|
||||
func = eval("lib.%s" % name, {"lib": lib})
|
||||
|
||||
def ret(lst1, fl, out):
|
||||
func(ffi.cast("double *", lst1.ctypes.data),
|
||||
ffi.cast("double", fl),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return ret
|
||||
|
||||
self.f = wrap_1list_1float("f_fun")
|
||||
self.F = wrap_1list_1float("F_fun")
|
||||
|
||||
self.err_function = wrap_2lists("err_fun")
|
||||
self.inv_err_function = wrap_2lists("inv_err_fun")
|
||||
self.H_mod = wrap_1lists("H_mod_fun")
|
||||
|
||||
self.hs, self.Hs, self.Hes = {}, {}, {}
|
||||
for kind in kinds:
|
||||
self.hs[kind] = wrap_2lists("h_%d" % kind)
|
||||
self.Hs[kind] = wrap_2lists("H_%d" % kind)
|
||||
if self.msckf and kind in self.feature_track_kinds:
|
||||
self.Hes[kind] = wrap_2lists("He_%d" % kind)
|
||||
|
||||
# wrap the C++ predict function
|
||||
def _predict_blas(x, P, dt):
|
||||
lib.predict(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", P.ctypes.data),
|
||||
ffi.cast("double *", self.Q.ctypes.data),
|
||||
ffi.cast("double", dt))
|
||||
return x, P
|
||||
|
||||
# wrap the C++ update function
|
||||
def fun_wrapper(f, kind):
|
||||
f = eval("lib.%s" % f, {"lib": lib})
|
||||
|
||||
def _update_inner_blas(x, P, z, R, extra_args):
|
||||
f(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", P.ctypes.data),
|
||||
ffi.cast("double *", z.ctypes.data),
|
||||
ffi.cast("double *", R.ctypes.data),
|
||||
ffi.cast("double *", extra_args.ctypes.data))
|
||||
if self.msckf and kind in self.feature_track_kinds:
|
||||
y = z[:-len(extra_args)]
|
||||
else:
|
||||
y = z
|
||||
return x, P, y
|
||||
return _update_inner_blas
|
||||
|
||||
self._updates = {}
|
||||
for kind in kinds:
|
||||
self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
|
||||
|
||||
def _update_blas(x, P, kind, z, R, extra_args=[]):
|
||||
return self._updates[kind](x, P, z, R, extra_args)
|
||||
|
||||
# assign the functions
|
||||
self._predict = _predict_blas
|
||||
# self._predict = self._predict_python
|
||||
self._update = _update_blas
|
||||
# self._update = self._update_python
|
||||
|
||||
def init_state(self, state, covs, filter_time):
|
||||
self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
|
||||
self.P = np.array(covs).astype(np.float64)
|
||||
self.filter_time = filter_time
|
||||
self.augment_times = [0] * self.N
|
||||
self.rewind_obscache = []
|
||||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
|
||||
def augment(self):
|
||||
# TODO this is not a generalized way of doing this and implies that the augmented states
|
||||
# are simply the first (dim_augment_state) elements of the main state.
|
||||
assert self.msckf
|
||||
d1 = self.dim_main
|
||||
d2 = self.dim_main_err
|
||||
d3 = self.dim_augment
|
||||
d4 = self.dim_augment_err
|
||||
|
||||
# push through augmented states
|
||||
self.x[d1:-d3] = self.x[d1 + d3:]
|
||||
self.x[-d3:] = self.x[:d3]
|
||||
assert self.x.shape == (self.dim_x, 1)
|
||||
|
||||
# push through augmented covs
|
||||
assert self.P.shape == (self.dim_err, self.dim_err)
|
||||
P_reduced = self.P
|
||||
P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=1)
|
||||
P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=0)
|
||||
assert P_reduced.shape == (self.dim_err - d4, self.dim_err - d4)
|
||||
to_mult = np.zeros((self.dim_err, self.dim_err - d4))
|
||||
to_mult[:-d4, :] = np.eye(self.dim_err - d4)
|
||||
to_mult[-d4:, :d4] = np.eye(d4)
|
||||
self.P = to_mult.dot(P_reduced.dot(to_mult.T))
|
||||
self.augment_times = self.augment_times[1:]
|
||||
self.augment_times.append(self.filter_time)
|
||||
assert self.P.shape == (self.dim_err, self.dim_err)
|
||||
|
||||
def state(self):
|
||||
return np.array(self.x).flatten()
|
||||
|
||||
def covs(self):
|
||||
return self.P
|
||||
|
||||
def rewind(self, t):
|
||||
# find where we are rewinding to
|
||||
idx = bisect_right(self.rewind_t, t)
|
||||
assert self.rewind_t[idx - 1] <= t
|
||||
assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
|
||||
|
||||
# set the state to the time right before that
|
||||
self.filter_time = self.rewind_t[idx - 1]
|
||||
self.x[:] = self.rewind_states[idx - 1][0]
|
||||
self.P[:] = self.rewind_states[idx - 1][1]
|
||||
|
||||
# return the observations we rewound over for fast forwarding
|
||||
ret = self.rewind_obscache[idx:]
|
||||
|
||||
# throw away the old future
|
||||
# TODO: is this making a copy?
|
||||
self.rewind_t = self.rewind_t[:idx]
|
||||
self.rewind_states = self.rewind_states[:idx]
|
||||
self.rewind_obscache = self.rewind_obscache[:idx]
|
||||
|
||||
return ret
|
||||
|
||||
def checkpoint(self, obs):
|
||||
# push to rewinder
|
||||
self.rewind_t.append(self.filter_time)
|
||||
self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
|
||||
self.rewind_obscache.append(obs)
|
||||
|
||||
# only keep a certain number around
|
||||
REWIND_TO_KEEP = 512
|
||||
self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
|
||||
self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
|
||||
self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
|
||||
|
||||
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):
|
||||
# TODO handle rewinding at this level"
|
||||
|
||||
# rewind
|
||||
if self.filter_time is not None and t < self.filter_time:
|
||||
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - 1.0:
|
||||
print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
|
||||
return None
|
||||
rewound = self.rewind(t)
|
||||
else:
|
||||
rewound = []
|
||||
|
||||
ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
|
||||
|
||||
# optional fast forward
|
||||
for r in rewound:
|
||||
self._predict_and_update_batch(*r)
|
||||
|
||||
return ret
|
||||
|
||||
def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
|
||||
"""The main kalman filter function
|
||||
Predicts the state and then updates a batch of observations
|
||||
|
||||
dim_x: dimensionality of the state space
|
||||
dim_z: dimensionality of the observation and depends on kind
|
||||
n: number of observations
|
||||
|
||||
Args:
|
||||
t (float): Time of observation
|
||||
kind (int): Type of observation
|
||||
z (vec [n,dim_z]): Measurements
|
||||
R (mat [n,dim_z, dim_z]): Measurement Noise
|
||||
extra_args (list, [n]): Values used in H computations
|
||||
"""
|
||||
# initialize time
|
||||
if self.filter_time is None:
|
||||
self.filter_time = t
|
||||
|
||||
# predict
|
||||
dt = t - self.filter_time
|
||||
assert dt >= 0
|
||||
self.x, self.P = self._predict(self.x, self.P, dt)
|
||||
self.filter_time = t
|
||||
xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
|
||||
|
||||
# update batch
|
||||
y = []
|
||||
for i in range(len(z)):
|
||||
# these are from the user, so we canonicalize them
|
||||
z_i = np.array(z[i], dtype=np.float64, order='F')
|
||||
R_i = np.array(R[i], dtype=np.float64, order='F')
|
||||
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
|
||||
# update
|
||||
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
|
||||
y.append(y_i)
|
||||
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
|
||||
|
||||
if augment:
|
||||
self.augment()
|
||||
|
||||
# checkpoint
|
||||
self.checkpoint((t, kind, z, R, extra_args))
|
||||
|
||||
return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
|
||||
|
||||
def _predict_python(self, x, P, dt):
|
||||
x_new = np.zeros(x.shape, dtype=np.float64)
|
||||
self.f(x, dt, x_new)
|
||||
|
||||
F = np.zeros(P.shape, dtype=np.float64)
|
||||
self.F(x, dt, F)
|
||||
|
||||
if not self.msckf:
|
||||
P = dot(dot(F, P), F.T)
|
||||
else:
|
||||
# Update the predicted state covariance:
|
||||
# Pk+1|k = |F*Pii*FT + Q*dt F*Pij |
|
||||
# |PijT*FT Pjj |
|
||||
# Where F is the jacobian of the main state
|
||||
# predict function, Pii is the main state's
|
||||
# covariance and Q its process noise. Pij
|
||||
# is the covariance between the augmented
|
||||
# states and the main state.
|
||||
#
|
||||
d2 = self.dim_main_err # known at compile time
|
||||
F_curr = F[:d2, :d2]
|
||||
P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
|
||||
P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
|
||||
P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
|
||||
|
||||
P += dt * self.Q
|
||||
return x_new, P
|
||||
|
||||
def _update_python(self, x, P, kind, z, R, extra_args=[]):
|
||||
# init vars
|
||||
z = z.reshape((-1, 1))
|
||||
h = np.zeros(z.shape, dtype=np.float64)
|
||||
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
|
||||
|
||||
# C functions
|
||||
self.hs[kind](x, extra_args, h)
|
||||
self.Hs[kind](x, extra_args, H)
|
||||
|
||||
# y is the "loss"
|
||||
y = z - h
|
||||
|
||||
# *** same above this line ***
|
||||
|
||||
if self.msckf and kind in self.Hes:
|
||||
# Do some algebraic magic to decorrelate
|
||||
He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
|
||||
self.Hes[kind](x, extra_args, He)
|
||||
|
||||
# TODO: Don't call a function here, do projection locally
|
||||
A = null(He.T)
|
||||
|
||||
y = A.T.dot(y)
|
||||
H = A.T.dot(H)
|
||||
R = A.T.dot(R.dot(A))
|
||||
|
||||
# TODO If nullspace isn't the dimension we want
|
||||
if A.shape[1] + He.shape[1] != A.shape[0]:
|
||||
print('Warning: null space projection failed, measurement ignored')
|
||||
return x, P, np.zeros(A.shape[0] - He.shape[1])
|
||||
|
||||
# if using eskf
|
||||
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
|
||||
self.H_mod(x, H_mod)
|
||||
H = H.dot(H_mod)
|
||||
|
||||
# Do mahalobis distance test
|
||||
# currently just runs on msckf observations
|
||||
# could run on anything if needed
|
||||
if self.msckf and kind in self.maha_test_kinds:
|
||||
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
|
||||
maha_dist = y.T.dot(a.dot(y))
|
||||
if maha_dist > chi2_ppf(0.95, y.shape[0]):
|
||||
R = 10e16 * R
|
||||
|
||||
# *** same below this line ***
|
||||
|
||||
# Outlier resilient weighting as described in:
|
||||
# "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
|
||||
weight = 1 # (1.5)/(1 + np.sum(y**2)/np.sum(R))
|
||||
|
||||
S = dot(dot(H, P), H.T) + R / weight
|
||||
K = solve(S, dot(H, P.T)).T
|
||||
I_KH = np.eye(P.shape[0]) - dot(K, H)
|
||||
|
||||
# update actual state
|
||||
delta_x = dot(K, y)
|
||||
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
|
||||
|
||||
# inject observed error into state
|
||||
x_new = np.zeros(x.shape, dtype=np.float64)
|
||||
self.err_function(x, delta_x, x_new)
|
||||
return x_new, P, y.flatten()
|
||||
|
||||
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
|
||||
# init vars
|
||||
z = z.reshape((-1, 1))
|
||||
h = np.zeros(z.shape, dtype=np.float64)
|
||||
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
|
||||
|
||||
# C functions
|
||||
self.hs[kind](x, extra_args, h)
|
||||
self.Hs[kind](x, extra_args, H)
|
||||
|
||||
# y is the "loss"
|
||||
y = z - h
|
||||
|
||||
# if using eskf
|
||||
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
|
||||
self.H_mod(x, H_mod)
|
||||
H = H.dot(H_mod)
|
||||
|
||||
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
|
||||
maha_dist = y.T.dot(a.dot(y))
|
||||
if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
def rts_smooth(self, estimates, norm_quats=False):
|
||||
'''
|
||||
Returns rts smoothed results of
|
||||
kalman filter estimates
|
||||
|
||||
If the kalman state is augmented with
|
||||
old states only the main state is smoothed
|
||||
'''
|
||||
xk_n = estimates[-1][0]
|
||||
Pk_n = estimates[-1][2]
|
||||
Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
|
||||
|
||||
states_smoothed = [xk_n]
|
||||
covs_smoothed = [Pk_n]
|
||||
for k in range(len(estimates) - 2, -1, -1):
|
||||
xk1_n = xk_n
|
||||
if norm_quats:
|
||||
xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
|
||||
Pk1_n = Pk_n
|
||||
|
||||
xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
|
||||
_, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
|
||||
dt = t2 - t1
|
||||
self.F(xk_k, dt, Fk_1)
|
||||
|
||||
d1 = self.dim_main
|
||||
d2 = self.dim_main_err
|
||||
Ck = np.linalg.solve(Pk1_k[:d2, :d2], Fk_1[:d2, :d2].dot(Pk_k[:d2, :d2].T)).T
|
||||
xk_n = xk_k
|
||||
delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
|
||||
self.inv_err_function(xk1_k, xk1_n, delta_x)
|
||||
delta_x[:d2] = Ck.dot(delta_x[:d2])
|
||||
x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
|
||||
self.err_function(xk_k, delta_x, x_new)
|
||||
xk_n[:d1] = x_new[:d1, 0]
|
||||
Pk_n = Pk_k
|
||||
Pk_n[:d2, :d2] = Pk_k[:d2, :d2] + Ck.dot(Pk1_n[:d2, :d2] - Pk1_k[:d2, :d2]).dot(Ck.T)
|
||||
states_smoothed.append(xk_n)
|
||||
covs_smoothed.append(Pk_n)
|
||||
|
||||
return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]
|
|
@ -0,0 +1,158 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
|
||||
import common.transformations.orientation as orient
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_matrix_l
|
||||
|
||||
|
||||
def sane(track):
|
||||
img_pos = track[1:, 2:4]
|
||||
diffs_x = abs(img_pos[1:, 0] - img_pos[:-1, 0])
|
||||
diffs_y = abs(img_pos[1:, 1] - img_pos[:-1, 1])
|
||||
for i in range(1, len(diffs_x)):
|
||||
if ((diffs_x[i] > 0.05 or diffs_x[i - 1] > 0.05) and
|
||||
(diffs_x[i] > 2 * diffs_x[i - 1] or
|
||||
diffs_x[i] < .5 * diffs_x[i - 1])) or \
|
||||
((diffs_y[i] > 0.05 or diffs_y[i - 1] > 0.05) and
|
||||
(diffs_y[i] > 2 * diffs_y[i - 1] or
|
||||
diffs_y[i] < .5 * diffs_y[i - 1])):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
class FeatureHandler():
|
||||
name = 'feature_handler'
|
||||
|
||||
@staticmethod
|
||||
def generate_code(K=5):
|
||||
# Wrap c code for slow matching
|
||||
c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);"
|
||||
|
||||
c_code = "#include <math.h>\n"
|
||||
c_code += "#include <string.h>\n"
|
||||
c_code += "#define K %d\n" % K
|
||||
c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c")).read()
|
||||
|
||||
filename = f"{FeatureHandler.name}_{K}"
|
||||
write_code(filename, c_code, c_header)
|
||||
|
||||
def __init__(self, K=5):
|
||||
self.MAX_TRACKS = 6000
|
||||
self.K = K
|
||||
|
||||
# Array of tracks, each track has K 5D features preceded
|
||||
# by 5 params that inidicate [f_idx, last_idx, updated, complete, valid]
|
||||
# f_idx: idx of current last feature in track
|
||||
# idx of of last feature in frame
|
||||
# bool for whether this track has been update
|
||||
# bool for whether this track is complete
|
||||
# bool for whether this track is valid
|
||||
self.tracks = np.zeros((self.MAX_TRACKS, K + 1, 5))
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
name = f"{FeatureHandler.name}_{K}"
|
||||
ffi, lib = load_code(name)
|
||||
|
||||
def merge_features_c(tracks, features, empty_idxs):
|
||||
lib.merge_features(ffi.cast("double *", tracks.ctypes.data),
|
||||
ffi.cast("double *", features.ctypes.data),
|
||||
ffi.cast("long long *", empty_idxs.ctypes.data))
|
||||
|
||||
# self.merge_features = self.merge_features_python
|
||||
self.merge_features = merge_features_c
|
||||
|
||||
def reset(self):
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
def merge_features_python(self, tracks, features, empty_idxs):
|
||||
empty_idx = 0
|
||||
for f in features:
|
||||
match_idx = int(f[4])
|
||||
if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0, 2] == 0:
|
||||
tracks[match_idx, 0, 0] += 1
|
||||
tracks[match_idx, 0, 1] = f[1]
|
||||
tracks[match_idx, 0, 2] = 1
|
||||
tracks[match_idx, int(tracks[match_idx, 0, 0])] = f
|
||||
if tracks[match_idx, 0, 0] == self.K:
|
||||
tracks[match_idx, 0, 3] = 1
|
||||
if sane(tracks[match_idx]):
|
||||
tracks[match_idx, 0, 4] = 1
|
||||
else:
|
||||
if empty_idx == len(empty_idxs):
|
||||
print('need more empty space')
|
||||
continue
|
||||
tracks[empty_idxs[empty_idx], 0, 0] = 1
|
||||
tracks[empty_idxs[empty_idx], 0, 1] = f[1]
|
||||
tracks[empty_idxs[empty_idx], 0, 2] = 1
|
||||
tracks[empty_idxs[empty_idx], 1] = f
|
||||
empty_idx += 1
|
||||
|
||||
def update_tracks(self, features):
|
||||
last_idxs = np.copy(self.tracks[:, 0, 1])
|
||||
real = np.isfinite(last_idxs)
|
||||
self.tracks[last_idxs[real].astype(int)] = self.tracks[real]
|
||||
|
||||
mask = np.ones(self.MAX_TRACKS, np.bool)
|
||||
mask[last_idxs[real].astype(int)] = 0
|
||||
empty_idxs = np.arange(self.MAX_TRACKS)[mask]
|
||||
|
||||
self.tracks[empty_idxs] = np.nan
|
||||
self.tracks[:, 0, 2] = 0
|
||||
self.merge_features(self.tracks, features, empty_idxs)
|
||||
|
||||
def handle_features(self, features):
|
||||
self.update_tracks(features)
|
||||
valid_idxs = self.tracks[:, 0, 4] == 1
|
||||
complete_idxs = self.tracks[:, 0, 3] == 1
|
||||
stale_idxs = self.tracks[:, 0, 2] == 0
|
||||
valid_tracks = self.tracks[valid_idxs]
|
||||
self.tracks[complete_idxs] = np.nan
|
||||
self.tracks[stale_idxs] = np.nan
|
||||
return valid_tracks[:, 1:, :4].reshape((len(valid_tracks), self.K * 4))
|
||||
|
||||
|
||||
def generate_orient_error_jac(K):
|
||||
import sympy as sp
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_rotate
|
||||
|
||||
x_sym = sp.MatrixSymbol('abr', 3, 1)
|
||||
dtheta = sp.MatrixSymbol('dtheta', 3, 1)
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:, :] = sp.Matrix(0.5 * dtheta[0:3, :])
|
||||
poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0])
|
||||
q = quat_matrix_l(poses_sym[K * 7 - 4:K * 7]) * delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c * quat_rot.T
|
||||
rows = []
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0])
|
||||
q = quat_matrix_l(poses_sym[7 * i + 3:7 * i + 7]) * delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c * quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i * (rot_g_to_0.T)
|
||||
trans_0_to_i = rot_g_to_i * (pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1 / h3 - img_pos_sym[i * 2 + 0])
|
||||
rows.append(h2 / h3 - img_pos_sym[i * 2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# TODO: get K from argparse
|
||||
FeatureHandler.generate_code()
|
|
@ -0,0 +1,176 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
import common.transformations.orientation as orient
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (quat_rotate,
|
||||
sympy_into_c)
|
||||
|
||||
|
||||
def generate_residual(K):
|
||||
x_sym = sp.MatrixSymbol('abr', 3, 1)
|
||||
poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0])
|
||||
q = poses_sym[K * 7 - 4:K * 7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c * quat_rot.T
|
||||
rows = []
|
||||
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0])
|
||||
q = poses_sym[7 * i + 3:7 * i + 7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c * quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i * rot_g_to_0.T
|
||||
trans_0_to_i = rot_g_to_i * (pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1 / h3 - img_pos_sym[i * 2 + 0])
|
||||
rows.append(h2 / h3 - img_pos_sym[i * 2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym]))
|
||||
sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
class LstSqComputer():
|
||||
name = 'pos_computer'
|
||||
|
||||
@staticmethod
|
||||
def generate_code(K=4):
|
||||
sympy_functions = generate_residual(K)
|
||||
header, code = sympy_into_c(sympy_functions)
|
||||
|
||||
code += "\n#define KDIM %d\n" % K
|
||||
code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read()
|
||||
|
||||
header += """
|
||||
void compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);
|
||||
"""
|
||||
|
||||
filename = f"{LstSqComputer.name}_{K}"
|
||||
write_code(filename, code, header)
|
||||
|
||||
def __init__(self, K=4, MIN_DEPTH=2, MAX_DEPTH=500):
|
||||
self.to_c = orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0)
|
||||
self.MAX_DEPTH = MAX_DEPTH
|
||||
self.MIN_DEPTH = MIN_DEPTH
|
||||
|
||||
name = f"{LstSqComputer.name}_{K}"
|
||||
ffi, lib = load_code(name)
|
||||
|
||||
# wrap c functions
|
||||
def residual_jac(x, poses, img_positions):
|
||||
out = np.zeros(((K * 2, 3)), dtype=np.float64)
|
||||
lib.jac_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual_jac = residual_jac
|
||||
|
||||
def residual(x, poses, img_positions):
|
||||
out = np.zeros((K * 2), dtype=np.float64)
|
||||
lib.res_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual = residual
|
||||
|
||||
def compute_pos_c(poses, img_positions):
|
||||
pos = np.zeros(3, dtype=np.float64)
|
||||
param = np.zeros(3, dtype=np.float64)
|
||||
# Can't be a view for the ctype
|
||||
img_positions = np.copy(img_positions)
|
||||
lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", param.ctypes.data),
|
||||
ffi.cast("double *", pos.ctypes.data))
|
||||
return pos, param
|
||||
self.compute_pos_c = compute_pos_c
|
||||
|
||||
def compute_pos(self, poses, img_positions, debug=False):
|
||||
pos, param = self.compute_pos_c(poses, img_positions)
|
||||
# pos, param = self.compute_pos_python(poses, img_positions)
|
||||
|
||||
depth = 1 / param[2]
|
||||
if debug:
|
||||
# orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3))
|
||||
jac = self.residual_jac(param, poses, img_positions).reshape((-1, 2, 3))
|
||||
res = self.residual(param, poses, img_positions).reshape((-1, 2))
|
||||
return pos, param, res, jac # , orient_err_jac
|
||||
elif (self.MIN_DEPTH < depth < self.MAX_DEPTH):
|
||||
return pos
|
||||
else:
|
||||
return None
|
||||
|
||||
def gauss_newton(self, fun, jac, x, args):
|
||||
poses, img_positions = args
|
||||
delta = 1
|
||||
counter = 0
|
||||
while abs(np.linalg.norm(delta)) > 1e-4 and counter < 30:
|
||||
delta = np.linalg.pinv(jac(x, poses, img_positions)).dot(fun(x, poses, img_positions))
|
||||
x = x - delta
|
||||
counter += 1
|
||||
return [x]
|
||||
|
||||
def compute_pos_python(self, poses, img_positions, check_quality=False):
|
||||
import scipy.optimize as opt
|
||||
|
||||
# This procedure is also described
|
||||
# in the MSCKF paper (Mourikis et al. 2007)
|
||||
x = np.array([img_positions[-1][0],
|
||||
img_positions[-1][1], 0.1])
|
||||
res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt
|
||||
# res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton
|
||||
|
||||
alpha, beta, rho = res[0]
|
||||
rot_0_to_g = (orient.rotations_from_quats(poses[-1, 3:])).dot(self.to_c.T)
|
||||
return (rot_0_to_g.dot(np.array([alpha, beta, 1]))) / rho + poses[-1, :3]
|
||||
|
||||
|
||||
# EXPERIMENTAL CODE
|
||||
def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
|
||||
# only speed correction for now
|
||||
t_roll = 0.016 # 16ms rolling shutter?
|
||||
vroll, vpitch, vyaw = rot_rates
|
||||
A = 0.5 * np.array([[-1, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A.dot(poses[-1][3:7])
|
||||
v = np.append(v, q_dot)
|
||||
v = np.array([v[0], v[1], v[2], 0, 0, 0, 0])
|
||||
current_pose = poses[-1] + v * 0.05
|
||||
poses = np.vstack((current_pose, poses))
|
||||
dt = -img_positions[:, 1] * t_roll / 0.48
|
||||
errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos)
|
||||
return img_positions - errs
|
||||
|
||||
|
||||
def project(poses, ecef_pos):
|
||||
img_positions = np.zeros((len(poses), 2))
|
||||
for i, p in enumerate(poses):
|
||||
cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3])
|
||||
img_positions[i] = np.array([cam_frame[1] / cam_frame[0], cam_frame[2] / cam_frame[0]])
|
||||
return img_positions
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
K = int(sys.argv[1].split("_")[-1])
|
||||
LstSqComputer.generate_code(K=K)
|
||||
|
|
@ -2,31 +2,35 @@
|
|||
import sympy as sp
|
||||
import numpy as np
|
||||
|
||||
|
||||
def cross(x):
|
||||
ret = sp.Matrix(np.zeros((3,3)))
|
||||
ret[0,1], ret[0,2] = -x[2], x[1]
|
||||
ret[1,0], ret[1,2] = x[2], -x[0]
|
||||
ret[2,0], ret[2,1] = -x[1], x[0]
|
||||
ret = sp.Matrix(np.zeros((3, 3)))
|
||||
ret[0, 1], ret[0, 2] = -x[2], x[1]
|
||||
ret[1, 0], ret[1, 2] = x[2], -x[0]
|
||||
ret[2, 0], ret[2, 1] = -x[1], x[0]
|
||||
return ret
|
||||
|
||||
|
||||
def euler_rotate(roll, pitch, yaw):
|
||||
# make symbolic rotation matrix from eulers
|
||||
matrix_roll = sp.Matrix([[1, 0, 0],
|
||||
[0, sp.cos(roll), -sp.sin(roll)],
|
||||
[0, sp.sin(roll), sp.cos(roll)]])
|
||||
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
|
||||
[0, 1, 0],
|
||||
[-sp.sin(pitch), 0, sp.cos(pitch)]])
|
||||
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
|
||||
[sp.sin(yaw), sp.cos(yaw), 0],
|
||||
[0, 0, 1]])
|
||||
return matrix_yaw*matrix_pitch*matrix_roll
|
||||
matrix_roll = sp.Matrix([[1, 0, 0],
|
||||
[0, sp.cos(roll), -sp.sin(roll)],
|
||||
[0, sp.sin(roll), sp.cos(roll)]])
|
||||
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
|
||||
[0, 1, 0],
|
||||
[-sp.sin(pitch), 0, sp.cos(pitch)]])
|
||||
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
|
||||
[sp.sin(yaw), sp.cos(yaw), 0],
|
||||
[0, 0, 1]])
|
||||
return matrix_yaw * matrix_pitch * matrix_roll
|
||||
|
||||
|
||||
def quat_rotate(q0, q1, q2, q3):
|
||||
# make symbolic rotation matrix from quat
|
||||
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2)],
|
||||
[2*(q1*q2 - q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 + q0*q1)],
|
||||
[2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
|
||||
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 + q0 * q3), 2 * (q1 * q3 - q0 * q2)],
|
||||
[2 * (q1 * q2 - q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 + q0 * q1)],
|
||||
[2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
|
||||
|
||||
|
||||
def quat_matrix_l(p):
|
||||
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
|
||||
|
@ -34,6 +38,7 @@ def quat_matrix_l(p):
|
|||
[p[2], p[3], p[0], -p[1]],
|
||||
[p[3], -p[2], p[1], p[0]]])
|
||||
|
||||
|
||||
def quat_matrix_r(p):
|
||||
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
|
||||
[p[1], p[0], p[3], -p[2]],
|
||||
|
@ -49,10 +54,11 @@ def sympy_into_c(sympy_functions):
|
|||
|
||||
# argument ordering input to sympy is broken with function with output arguments
|
||||
nargs = []
|
||||
|
||||
# reorder the input arguments
|
||||
for aa in args:
|
||||
if aa is None:
|
||||
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1,1]))
|
||||
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1, 1]))
|
||||
continue
|
||||
found = False
|
||||
for a in r.arguments:
|
||||
|
@ -62,20 +68,23 @@ def sympy_into_c(sympy_functions):
|
|||
break
|
||||
if not found:
|
||||
# [1,1] is a hack for Matrices
|
||||
nargs.append(codegen.InputArgument(aa, dimensions=[1,1]))
|
||||
nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
|
||||
|
||||
# add the output arguments
|
||||
for a in r.arguments:
|
||||
if type(a) == codegen.OutputArgument:
|
||||
nargs.append(a)
|
||||
|
||||
#assert len(r.arguments) == len(args)+1
|
||||
# assert len(r.arguments) == len(args)+1
|
||||
r.arguments = nargs
|
||||
|
||||
# add routine to list
|
||||
routines.append(r)
|
||||
|
||||
[(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
|
||||
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
|
||||
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
|
||||
|
||||
return c_header, c_code
|
|
@ -0,0 +1,284 @@
|
|||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from selfdrive.locationd.kalman.helpers import KalmanError, ObservationKind
|
||||
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
|
||||
quat_matrix_r,
|
||||
quat_rotate)
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
|
||||
ODO_SCALE = slice(16, 17) # odometer scale
|
||||
ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
|
||||
IMU_OFFSET = slice(20, 23) # imu offset angles in radians
|
||||
|
||||
# Error-state has different slices because it is an ESKF
|
||||
ECEF_POS_ERR = slice(0, 3)
|
||||
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
|
||||
ECEF_VELOCITY_ERR = slice(6, 9)
|
||||
ANGULAR_VELOCITY_ERR = slice(9, 12)
|
||||
GYRO_BIAS_ERR = slice(12, 15)
|
||||
ODO_SCALE_ERR = slice(15, 16)
|
||||
ACCELERATION_ERR = slice(16, 19)
|
||||
IMU_OFFSET_ERR = slice(19, 22)
|
||||
|
||||
|
||||
class LiveKalman():
|
||||
name = 'live'
|
||||
|
||||
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
0, 0, 0])
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
10**2, 10**2, 10**2,
|
||||
1**2, 1**2, 1**2,
|
||||
0.05**2, 0.05**2, 0.05**2,
|
||||
0.02**2,
|
||||
1**2, 1**2, 1**2,
|
||||
(0.01)**2, (0.01)**2, (0.01)**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
|
||||
(0.02 / 100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
|
||||
|
||||
@staticmethod
|
||||
def generate_code():
|
||||
name = LiveKalman.name
|
||||
dim_state = LiveKalman.initial_x.shape[0]
|
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0]
|
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x, y, z = state[States.ECEF_POS, :]
|
||||
q = state[States.ECEF_ORIENTATION, :]
|
||||
v = state[States.ECEF_VELOCITY, :]
|
||||
vx, vy, vz = v
|
||||
omega = state[States.ANGULAR_VELOCITY, :]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
|
||||
odo_scale = state[States.ODO_SCALE, :][0,:]
|
||||
acceleration = state[States.ACCELERATION, :]
|
||||
imu_angles = state[States.IMU_OFFSET, :]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.ECEF_POS, :] = v
|
||||
state_dot[States.ECEF_ORIENTATION, :] = q_dot
|
||||
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
|
||||
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
|
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
|
||||
acceleration_err = state_err[States.ACCELERATION_ERR, :]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[States.ECEF_POS_ERR, :] = v_err
|
||||
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
|
||||
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
f_err_sym = state_err + dt * state_err_dot
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
|
||||
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
|
||||
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
|
||||
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
|
||||
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
delta_quat = sp.Matrix(np.ones((4)))
|
||||
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
|
||||
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
|
||||
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
|
||||
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
|
||||
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
|
||||
h_acc_sym = imu_rot * (gravity + acceleration)
|
||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
|
||||
|
||||
speed = sp.sqrt(vx**2 + vy**2 + vz**2)
|
||||
h_speed_sym = sp.Matrix([speed * odo_scale])
|
||||
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_imu_frame_sym = sp.Matrix(imu_angles)
|
||||
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
|
||||
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
|
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
|
||||
|
||||
def __init__(self):
|
||||
self.dim_state = self.initial_x.shape[0]
|
||||
self.dim_state_err = self.initial_P_diag.shape[0]
|
||||
|
||||
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
|
||||
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
|
||||
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return self.filter.state()
|
||||
|
||||
@property
|
||||
def t(self):
|
||||
return self.filter.filter_time
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
return self.filter.covs()
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=True)
|
||||
|
||||
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
|
||||
if covs_diag is not None:
|
||||
P = np.diag(covs_diag)
|
||||
elif covs is not None:
|
||||
P = covs
|
||||
else:
|
||||
P = self.filter.covs()
|
||||
self.filter.init_state(state, P, filter_time)
|
||||
|
||||
def predict_and_observe(self, t, kind, data):
|
||||
if len(data) > 0:
|
||||
data = np.atleast_2d(data)
|
||||
if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
|
||||
r = self.predict_and_update_odo_trans(data, t, kind)
|
||||
elif kind == ObservationKind.CAMERA_ODO_ROTATION:
|
||||
r = self.predict_and_update_odo_rot(data, t, kind)
|
||||
elif kind == ObservationKind.ODOMETRIC_SPEED:
|
||||
r = self.predict_and_update_odo_speed(data, t, kind)
|
||||
else:
|
||||
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
|
||||
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
|
||||
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not (0.1 < quat_norm < 10):
|
||||
cloudlog.error("Kalman filter quaternions unstable")
|
||||
raise KalmanError
|
||||
|
||||
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
|
||||
|
||||
return r
|
||||
|
||||
def get_R(self, kind, n):
|
||||
obs_noise = self.obs_noise[kind]
|
||||
dim = obs_noise.shape[0]
|
||||
R = np.zeros((n, dim, dim))
|
||||
for i in range(n):
|
||||
R[i, :, :] = obs_noise
|
||||
return R
|
||||
|
||||
def predict_and_update_odo_speed(self, speed, t, kind):
|
||||
z = np.array(speed)
|
||||
R = np.zeros((len(speed), 1, 1))
|
||||
for i, _ in enumerate(z):
|
||||
R[i, :, :] = np.diag([0.2**2])
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R)
|
||||
|
||||
def predict_and_update_odo_trans(self, trans, t, kind):
|
||||
z = trans[:, :3]
|
||||
R = np.zeros((len(trans), 3, 3))
|
||||
for i, _ in enumerate(z):
|
||||
R[i, :, :] = np.diag(trans[i, 3:]**2)
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R)
|
||||
|
||||
def predict_and_update_odo_rot(self, rot, t, kind):
|
||||
z = rot[:, :3]
|
||||
R = np.zeros((len(rot), 3, 3))
|
||||
for i, _ in enumerate(z):
|
||||
R[i, :, :] = np.diag(rot[i, 3:]**2)
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
LiveKalman.generate_code()
|
|
@ -0,0 +1,54 @@
|
|||
#include <Eigen/QR>
|
||||
#include <Eigen/Dense>
|
||||
#include <iostream>
|
||||
|
||||
typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
|
||||
typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
|
||||
typedef Eigen::Matrix<double, 3, 1> O1M;
|
||||
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
|
||||
|
||||
extern "C" {
|
||||
void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
|
||||
|
||||
double res[KDIM*2] = {0};
|
||||
double jac[KDIM*6] = {0};
|
||||
|
||||
O1M x(in_x);
|
||||
O1M delta;
|
||||
int counter = 0;
|
||||
while ((delta.squaredNorm() > 0.0001 and counter < 30) or counter == 0){
|
||||
res_fun(in_x, in_poses, in_img_positions, res);
|
||||
jac_fun(in_x, in_poses, in_img_positions, jac);
|
||||
R1M E(res); R3M J(jac);
|
||||
delta = (J.transpose()*J).inverse() * J.transpose() * E;
|
||||
x = x - delta;
|
||||
memcpy(in_x, x.data(), 3 * sizeof(double));
|
||||
counter = counter + 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) {
|
||||
param[0] = img_positions[KDIM*2-2];
|
||||
param[1] = img_positions[KDIM*2-1];
|
||||
param[2] = 0.1;
|
||||
gauss_newton(param, poses, img_positions);
|
||||
|
||||
Eigen::Quaterniond q;
|
||||
q.w() = poses[KDIM*7-4];
|
||||
q.x() = poses[KDIM*7-3];
|
||||
q.y() = poses[KDIM*7-2];
|
||||
q.z() = poses[KDIM*7-1];
|
||||
M3D RC(to_c);
|
||||
Eigen::Matrix3d R = q.normalized().toRotationMatrix();
|
||||
Eigen::Matrix3d rot = R * RC.transpose();
|
||||
|
||||
pos[0] = param[0]/param[2];
|
||||
pos[1] = param[1]/param[2];
|
||||
pos[2] = 1.0/param[2];
|
||||
O1M ecef_offset(poses + KDIM*7-7);
|
||||
O1M ecef_output(pos);
|
||||
ecef_output = rot*ecef_output + ecef_offset;
|
||||
memcpy(pos, ecef_output.data(), 3 * sizeof(double));
|
||||
}
|
||||
}
|
|
@ -0,0 +1,123 @@
|
|||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
|
||||
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
|
||||
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
|
||||
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
|
||||
|
||||
void predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
|
||||
|
||||
double nx[DIM] = {0};
|
||||
double in_F[EDIM*EDIM] = {0};
|
||||
|
||||
// functions from sympy
|
||||
f_fun(in_x, dt, nx);
|
||||
F_fun(in_x, dt, in_F);
|
||||
|
||||
|
||||
EEM F(in_F);
|
||||
EEM P(in_P);
|
||||
EEM Q(in_Q);
|
||||
|
||||
RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
|
||||
P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
|
||||
P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
|
||||
P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
|
||||
|
||||
P = P + dt*Q;
|
||||
|
||||
// copy out state
|
||||
memcpy(in_x, nx, DIM * sizeof(double));
|
||||
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
|
||||
}
|
||||
|
||||
// note: extra_args dim only correct when null space projecting
|
||||
// otherwise 1
|
||||
template <int ZDIM, int EADIM, bool MAHA_TEST>
|
||||
void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
|
||||
typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
|
||||
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
|
||||
//typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
|
||||
|
||||
double in_hx[ZDIM] = {0};
|
||||
double in_H[ZDIM * DIM] = {0};
|
||||
double in_H_mod[EDIM * DIM] = {0};
|
||||
double delta_x[EDIM] = {0};
|
||||
double x_new[DIM] = {0};
|
||||
|
||||
|
||||
// state x, P
|
||||
Eigen::Matrix<double, ZDIM, 1> z(in_z);
|
||||
EEM P(in_P);
|
||||
ZZM pre_R(in_R);
|
||||
|
||||
// functions from sympy
|
||||
h_fun(in_x, in_ea, in_hx);
|
||||
H_fun(in_x, in_ea, in_H);
|
||||
ZDM pre_H(in_H);
|
||||
|
||||
// get y (y = z - hx)
|
||||
Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
|
||||
X1M y; XXM H; XXM R;
|
||||
if (Hea_fun){
|
||||
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
|
||||
double in_Hea[ZDIM * EADIM] = {0};
|
||||
Hea_fun(in_x, in_ea, in_Hea);
|
||||
ZAM Hea(in_Hea);
|
||||
XXM A = Hea.transpose().fullPivLu().kernel();
|
||||
|
||||
|
||||
y = A.transpose() * pre_y;
|
||||
H = A.transpose() * pre_H;
|
||||
R = A.transpose() * pre_R * A;
|
||||
} else {
|
||||
y = pre_y;
|
||||
H = pre_H;
|
||||
R = pre_R;
|
||||
}
|
||||
// get modified H
|
||||
H_mod_fun(in_x, in_H_mod);
|
||||
DEM H_mod(in_H_mod);
|
||||
XEM H_err = H * H_mod;
|
||||
|
||||
// Do mahalobis distance test
|
||||
if (MAHA_TEST){
|
||||
XXM a = (H_err * P * H_err.transpose() + R).inverse();
|
||||
double maha_dist = y.transpose() * a * y;
|
||||
if (maha_dist > MAHA_THRESHOLD){
|
||||
R = 1.0e16 * R;
|
||||
}
|
||||
}
|
||||
|
||||
// Outlier resilient weighting
|
||||
double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
|
||||
|
||||
// kalman gains and I_KH
|
||||
XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
|
||||
XEM KT = S.fullPivLu().solve(H_err * P.transpose());
|
||||
//EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
|
||||
//EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
|
||||
//std::cout << "Here is the matrix rot:\n" << K << std::endl;
|
||||
EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
|
||||
|
||||
// update state by injecting dx
|
||||
Eigen::Matrix<double, EDIM, 1> dx(delta_x);
|
||||
dx = (KT.transpose() * y);
|
||||
memcpy(delta_x, dx.data(), EDIM * sizeof(double));
|
||||
err_fun(in_x, delta_x, x_new);
|
||||
Eigen::Matrix<double, DIM, 1> x(x_new);
|
||||
|
||||
// update cov
|
||||
P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
|
||||
|
||||
// copy out state
|
||||
memcpy(in_x, x.data(), DIM * sizeof(double));
|
||||
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
|
||||
memcpy(in_z, y.data(), y.rows() * sizeof(double));
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
extern "C"{
|
||||
bool sane(double track [K + 1][5]) {
|
||||
double diffs_x [K-1];
|
||||
double diffs_y [K-1];
|
||||
int i;
|
||||
for (i = 0; i < K-1; i++) {
|
||||
diffs_x[i] = fabs(track[i+2][2] - track[i+1][2]);
|
||||
diffs_y[i] = fabs(track[i+2][3] - track[i+1][3]);
|
||||
}
|
||||
for (i = 1; i < K-1; i++) {
|
||||
if (((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and
|
||||
(diffs_x[i] > 2*diffs_x[i-1] or
|
||||
diffs_x[i] < .5*diffs_x[i-1])) or
|
||||
((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and
|
||||
(diffs_y[i] > 2*diffs_y[i-1] or
|
||||
diffs_y[i] < .5*diffs_y[i-1]))){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void merge_features(double *tracks, double *features, long long *empty_idxs) {
|
||||
double feature_arr [3000][5];
|
||||
memcpy(feature_arr, features, 3000 * 5 * sizeof(double));
|
||||
double track_arr [6000][K + 1][5];
|
||||
memcpy(track_arr, tracks, (K+1) * 6000 * 5 * sizeof(double));
|
||||
int match;
|
||||
int empty_idx = 0;
|
||||
int idx;
|
||||
for (int i = 0; i < 3000; i++) {
|
||||
match = feature_arr[i][4];
|
||||
if (track_arr[match][0][1] == match and track_arr[match][0][2] == 0){
|
||||
track_arr[match][0][0] = track_arr[match][0][0] + 1;
|
||||
track_arr[match][0][1] = feature_arr[i][1];
|
||||
track_arr[match][0][2] = 1;
|
||||
idx = track_arr[match][0][0];
|
||||
memcpy(track_arr[match][idx], feature_arr[i], 5 * sizeof(double));
|
||||
if (idx == K){
|
||||
// label complete
|
||||
track_arr[match][0][3] = 1;
|
||||
if (sane(track_arr[match])){
|
||||
// label valid
|
||||
track_arr[match][0][4] = 1;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// gen new track with this feature
|
||||
track_arr[empty_idxs[empty_idx]][0][0] = 1;
|
||||
track_arr[empty_idxs[empty_idx]][0][1] = feature_arr[i][1];
|
||||
track_arr[empty_idxs[empty_idx]][0][2] = 1;
|
||||
memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double));
|
||||
empty_idx = empty_idx + 1;
|
||||
}
|
||||
}
|
||||
memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double));
|
||||
}
|
||||
}
|
|
@ -0,0 +1,202 @@
|
|||
#!/usr/bin/env python3
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
import common.transformations.coordinates as coord
|
||||
from common.transformations.orientation import (ecef_euler_from_ned,
|
||||
euler2quat,
|
||||
ned_euler_from_ecef,
|
||||
quat2euler,
|
||||
rotations_from_quats)
|
||||
from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError
|
||||
from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
VISION_DECIMATION = 2
|
||||
SENSOR_DECIMATION = 10
|
||||
|
||||
|
||||
class Localizer():
|
||||
def __init__(self, disabled_logs=[], dog=None):
|
||||
self.kf = LiveKalman()
|
||||
self.reset_kalman()
|
||||
self.max_age = .2 # seconds
|
||||
self.disabled_logs = disabled_logs
|
||||
|
||||
def liveLocationMsg(self, time):
|
||||
fix = messaging.log.LiveLocationData.new_message()
|
||||
|
||||
predicted_state = self.kf.x
|
||||
|
||||
fix_ecef = predicted_state[States.ECEF_POS]
|
||||
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
|
||||
fix.lat = float(fix_pos_geo[0])
|
||||
fix.lon = float(fix_pos_geo[1])
|
||||
fix.alt = float(fix_pos_geo[2])
|
||||
|
||||
fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY]))
|
||||
|
||||
orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
|
||||
fix.roll = math.degrees(orientation_ned_euler[0])
|
||||
fix.pitch = math.degrees(orientation_ned_euler[1])
|
||||
fix.heading = math.degrees(orientation_ned_euler[2])
|
||||
|
||||
fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])]
|
||||
fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])]
|
||||
|
||||
ned_vel = self.converter.ecef2ned(predicted_state[States.ECEF_POS] + predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(predicted_state[States.ECEF_POS])
|
||||
fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])]
|
||||
fix.source = 'kalman'
|
||||
|
||||
#local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
|
||||
#fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0]))
|
||||
#fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0]))
|
||||
|
||||
imu_frame = predicted_state[States.IMU_OFFSET]
|
||||
fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])]
|
||||
return fix
|
||||
|
||||
def update_kalman(self, time, kind, meas):
|
||||
if self.filter_ready:
|
||||
try:
|
||||
self.kf.predict_and_observe(time, kind, meas)
|
||||
except KalmanError:
|
||||
cloudlog.error("Error in predict and observe, kalman reset")
|
||||
self.reset_kalman()
|
||||
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
|
||||
#self.observation_buffer.insert(idx, (time, kind, meas))
|
||||
#while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
|
||||
# else:
|
||||
# self.observation_buffer.pop(0)
|
||||
|
||||
def handle_gps(self, current_time, log):
|
||||
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
|
||||
fix_ecef = self.converter.ned2ecef([0, 0, 0])
|
||||
|
||||
# TODO initing with bad bearing not allowed, maybe not bad?
|
||||
if not self.filter_ready and log.speed > 5:
|
||||
self.filter_ready = True
|
||||
initial_ecef = fix_ecef
|
||||
gps_bearing = math.radians(log.bearing)
|
||||
initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
|
||||
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
|
||||
gps_speed = log.speed
|
||||
quat_uncertainty = 0.2**2
|
||||
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
|
||||
|
||||
initial_state = LiveKalman.initial_x
|
||||
initial_covs_diag = LiveKalman.initial_P_diag
|
||||
|
||||
initial_state[States.ECEF_POS] = initial_ecef
|
||||
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
|
||||
initial_state[States.ECEF_VELOCITY] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
|
||||
|
||||
initial_covs_diag[States.ECEF_POS_ERR] = 10**2
|
||||
initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
|
||||
initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
|
||||
self.kf.init_state(initial_state, covs=np.diag(initial_covs_diag), filter_time=current_time)
|
||||
cloudlog.info("Filter initialized")
|
||||
elif self.filter_ready:
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef)
|
||||
gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
|
||||
(self.kf.x[1] - fix_ecef[1])**2 +
|
||||
(self.kf.x[2] - fix_ecef[2])**2)
|
||||
if gps_est_error > 50:
|
||||
cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset")
|
||||
self.reset_kalman()
|
||||
|
||||
def handle_car_state(self, current_time, log):
|
||||
self.speed_counter += 1
|
||||
|
||||
if self.speed_counter % SENSOR_DECIMATION == 0:
|
||||
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo])
|
||||
if log.vEgo == 0:
|
||||
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
|
||||
|
||||
def handle_cam_odo(self, current_time, log):
|
||||
self.cam_counter += 1
|
||||
|
||||
if self.cam_counter % VISION_DECIMATION == 0:
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_ROTATION,
|
||||
np.concatenate([log.rot, log.rotStd]))
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_TRANSLATION,
|
||||
np.concatenate([log.trans, log.transStd]))
|
||||
|
||||
def handle_sensors(self, current_time, log):
|
||||
# TODO does not yet account for double sensor readings in the log
|
||||
for sensor_reading in log:
|
||||
# Gyro Uncalibrated
|
||||
if sensor_reading.sensor == 5 and sensor_reading.type == 16:
|
||||
self.gyro_counter += 1
|
||||
if self.gyro_counter % SENSOR_DECIMATION == 0:
|
||||
if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
|
||||
cloudlog.info('imu frame angles exceeded, correcting')
|
||||
self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0])
|
||||
|
||||
v = sensor_reading.gyroUncalibrated.v
|
||||
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
|
||||
|
||||
# Accelerometer
|
||||
if sensor_reading.sensor == 1 and sensor_reading.type == 1:
|
||||
self.acc_counter += 1
|
||||
if self.acc_counter % SENSOR_DECIMATION == 0:
|
||||
v = sensor_reading.acceleration.v
|
||||
self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
|
||||
|
||||
def reset_kalman(self):
|
||||
self.filter_time = None
|
||||
self.filter_ready = False
|
||||
self.observation_buffer = []
|
||||
|
||||
self.gyro_counter = 0
|
||||
self.acc_counter = 0
|
||||
self.speed_counter = 0
|
||||
self.cam_counter = 0
|
||||
|
||||
|
||||
def locationd_thread(sm, pm, disabled_logs=[]):
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveLocation'])
|
||||
|
||||
localizer = Localizer(disabled_logs=disabled_logs)
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
for sock, updated in sm.updated.items():
|
||||
if updated:
|
||||
t = sm.logMonoTime[sock] * 1e-9
|
||||
if sock == "sensorEvents":
|
||||
localizer.handle_sensors(t, sm[sock])
|
||||
elif sock == "gpsLocationExternal":
|
||||
localizer.handle_gps(t, sm[sock])
|
||||
elif sock == "carState":
|
||||
localizer.handle_car_state(t, sm[sock])
|
||||
elif sock == "cameraOdometry":
|
||||
localizer.handle_cam_odo(t, sm[sock])
|
||||
|
||||
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
|
||||
t = sm.logMonoTime['gpsLocationExternal']
|
||||
msg = messaging.new_message()
|
||||
msg.logMonoTime = t
|
||||
|
||||
msg.init('liveLocation')
|
||||
msg.liveLocation = localizer.liveLocationMsg(t * 1e-9)
|
||||
|
||||
pm.send('liveLocation', msg)
|
||||
|
||||
|
||||
def main(sm=None, pm=None):
|
||||
locationd_thread(sm, pm)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import os
|
||||
os.environ["OMP_NUM_THREADS"] = "1"
|
||||
main()
|
|
@ -3,7 +3,7 @@
|
|||
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <Eigen/Dense>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#define DEGREES_TO_RADIANS 0.017453292519943295
|
||||
|
|
|
@ -3,15 +3,19 @@ import os
|
|||
import shutil
|
||||
import threading
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.loggerd.config import ROOT, get_available_bytes
|
||||
from selfdrive.loggerd.config import ROOT, get_available_bytes, get_available_percent
|
||||
from selfdrive.loggerd.uploader import listdir_by_creation
|
||||
|
||||
MIN_BYTES = 5 * 1024 * 1024 * 1024
|
||||
MIN_PERCENT = 10
|
||||
|
||||
|
||||
def deleter_thread(exit_event):
|
||||
while not exit_event.is_set():
|
||||
available_bytes = get_available_bytes()
|
||||
out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES
|
||||
out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT
|
||||
|
||||
if available_bytes is not None and available_bytes < (5 * 1024 * 1024 * 1024):
|
||||
if out_of_percent or out_of_bytes:
|
||||
# remove the earliest directory we can
|
||||
dirs = listdir_by_creation(ROOT)
|
||||
for delete_dir in dirs:
|
||||
|
|
|
@ -588,7 +588,6 @@ int main(int argc, char** argv) {
|
|||
|
||||
for (const auto& it : services) {
|
||||
std::string name = it.name;
|
||||
int qlog_freq = it.decimation ? it.decimation : 0;
|
||||
|
||||
if (it.should_log) {
|
||||
SubSocket * sock = SubSocket::create(s.ctx, name);
|
||||
|
@ -601,8 +600,8 @@ int main(int argc, char** argv) {
|
|||
frame_sock = sock;
|
||||
}
|
||||
|
||||
qlog_counter[sock] = (qlog_freq == 0) ? -1 : 0;
|
||||
qlog_freqs[sock] = qlog_freq;
|
||||
qlog_counter[sock] = (it.decimation == -1) ? -1 : 0;
|
||||
qlog_freqs[sock] = it.decimation;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -14,7 +14,7 @@ from common.android import ANDROID
|
|||
sys.path.append(os.path.join(BASEDIR, "pyextra"))
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
TOTAL_SCONS_NODES = 1170
|
||||
TOTAL_SCONS_NODES = 1195
|
||||
prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
|
||||
|
||||
# Create folders needed for msgq
|
||||
|
@ -142,6 +142,7 @@ managed_processes = {
|
|||
"ubloxd": ("selfdrive/locationd", ["./ubloxd"]),
|
||||
"loggerd": ("selfdrive/loggerd", ["./loggerd"]),
|
||||
"logmessaged": "selfdrive.logmessaged",
|
||||
"locationd": "selfdrive.locationd.locationd",
|
||||
"tombstoned": "selfdrive.tombstoned",
|
||||
"logcatd": ("selfdrive/logcatd", ["./logcatd"]),
|
||||
"proclogd": ("selfdrive/proclogd", ["./proclogd"]),
|
||||
|
@ -204,6 +205,7 @@ car_started_processes = [
|
|||
'modeld',
|
||||
'proclogd',
|
||||
'ubloxd',
|
||||
'locationd',
|
||||
]
|
||||
if ANDROID:
|
||||
car_started_processes += [
|
||||
|
@ -289,6 +291,15 @@ def prepare_managed_process(p):
|
|||
subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0]))
|
||||
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0]))
|
||||
|
||||
|
||||
def join_process(process, timeout):
|
||||
# Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382
|
||||
# We have to poll the exitcode instead
|
||||
t = time.time()
|
||||
while time.time() - t < timeout and process.exitcode is None:
|
||||
time.sleep(0.001)
|
||||
|
||||
|
||||
def kill_managed_process(name):
|
||||
if name not in running or name not in managed_processes:
|
||||
return
|
||||
|
@ -302,18 +313,12 @@ def kill_managed_process(name):
|
|||
else:
|
||||
running[name].terminate()
|
||||
|
||||
# Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382
|
||||
# We have to poll the exitcode instead
|
||||
# running[name].join(5.0)
|
||||
|
||||
t = time.time()
|
||||
while time.time() - t < 5 and running[name].exitcode is None:
|
||||
time.sleep(0.001)
|
||||
join_process(running[name], 5)
|
||||
|
||||
if running[name].exitcode is None:
|
||||
if name in unkillable_processes:
|
||||
cloudlog.critical("unkillable process %s failed to exit! rebooting in 15 if it doesn't die" % name)
|
||||
running[name].join(15.0)
|
||||
join_process(running[name], 15)
|
||||
if running[name].exitcode is None:
|
||||
cloudlog.critical("FORCE REBOOTING PHONE!")
|
||||
os.system("date >> /sdcard/unkillable_reboot")
|
||||
|
@ -502,6 +507,8 @@ def main():
|
|||
params.put("LastUpdateTime", t.encode('utf8'))
|
||||
if params.get("OpenpilotEnabledToggle") is None:
|
||||
params.put("OpenpilotEnabledToggle", "1")
|
||||
if params.get("LaneChangeEnabled") is None:
|
||||
params.put("LaneChangeEnabled", "1")
|
||||
|
||||
# is this chffrplus?
|
||||
if os.getenv("PASSIVE") is not None:
|
||||
|
|
|
@ -1 +1 @@
|
|||
bc89e6f25e88a904ad905296d516aaebb77e2207
|
||||
917e6889be1691fb96e7566a92e0c6bbefc861a4
|
|
@ -58,7 +58,6 @@ def get_route_log(route_name):
|
|||
sys.exit(-1)
|
||||
|
||||
routes = {
|
||||
|
||||
"975b26878285314d|2018-12-25--14-42-13": {
|
||||
'carFingerprint': CHRYSLER.PACIFICA_2018_HYBRID,
|
||||
'enableCamera': True,
|
||||
|
@ -290,6 +289,7 @@ routes = {
|
|||
"7e34a988419b5307|2019-12-18--19-13-30": {
|
||||
'carFingerprint': TOYOTA.RAV4H_TSS2,
|
||||
'enableCamera': True,
|
||||
'fingerprintSource': 'fixed'
|
||||
},
|
||||
"e6a24be49a6cd46e|2019-10-29--10-52-42": {
|
||||
'carFingerprint': TOYOTA.LEXUS_ES_TSS2,
|
||||
|
@ -325,6 +325,11 @@ routes = {
|
|||
'carFingerprint': TOYOTA.LEXUS_RX_TSS2,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"ec429c0f37564e3c|2020-02-01--17-28-12": {
|
||||
'carFingerprint': TOYOTA.LEXUS_NXH,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
#FIXME: This works sometimes locally, but never in CI. Timing issue?
|
||||
#"b0f5a01cf604185c|2018-01-31--20-11-39": {
|
||||
# 'carFingerprint': TOYOTA.LEXUS_RXH,
|
||||
|
@ -368,6 +373,11 @@ routes = {
|
|||
# 'enableDsu': False,
|
||||
# },
|
||||
# TODO: missingsome combos for highlander
|
||||
"0a302ffddbb3e3d3|2020-02-08--16-19-08": {
|
||||
'carFingerprint': TOYOTA.HIGHLANDER_TSS2,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"aa659debdd1a7b54|2018-08-31--11-12-01": {
|
||||
'carFingerprint': TOYOTA.HIGHLANDER,
|
||||
'enableCamera': False,
|
||||
|
@ -507,6 +517,11 @@ if __name__ == "__main__":
|
|||
params.put("CommunityFeaturesToggle", "1")
|
||||
params.put("Passive", "1" if route in passive_routes else "0")
|
||||
|
||||
if checks.get('fingerprintSource', None) == 'fixed':
|
||||
os.environ['FINGERPRINT'] = checks['carFingerprint']
|
||||
else:
|
||||
os.environ['FINGERPRINT'] = ""
|
||||
|
||||
print("testing ", route, " ", checks['carFingerprint'])
|
||||
print("Starting processes")
|
||||
for p in tested_procs:
|
||||
|
|
|
@ -77,6 +77,10 @@ valid = True
|
|||
for idx1, f1 in enumerate(fingerprints_flat):
|
||||
for idx2, f2 in enumerate(fingerprints_flat):
|
||||
if idx1 < idx2 and not check_fingerprint_consistency(f1, f2):
|
||||
if car_names[idx1] == car_names[idx2]:
|
||||
print(f"Warning, overlap in {car_names[idx1]}")
|
||||
continue
|
||||
|
||||
valid = False
|
||||
print("Those two fingerprints are inconsistent {0} {1}".format(car_names[idx1], car_names[idx2]))
|
||||
print("")
|
||||
|
|
|
@ -105,118 +105,118 @@ def test_uploader():
|
|||
print("UPLOADER")
|
||||
time.sleep(10.0)
|
||||
|
||||
# @phone_only
|
||||
# def test_athena():
|
||||
# print("ATHENA")
|
||||
# start_daemon_process("manage_athenad")
|
||||
# params = Params()
|
||||
# manage_athenad_pid = params.get("AthenadPid")
|
||||
# assert manage_athenad_pid is not None
|
||||
# try:
|
||||
# os.kill(int(manage_athenad_pid), 0)
|
||||
# # process is running
|
||||
# except OSError:
|
||||
# assert False, "manage_athenad is dead"
|
||||
@phone_only
|
||||
def test_athena():
|
||||
print("ATHENA")
|
||||
start_daemon_process("manage_athenad")
|
||||
params = Params()
|
||||
manage_athenad_pid = params.get("AthenadPid")
|
||||
assert manage_athenad_pid is not None
|
||||
try:
|
||||
os.kill(int(manage_athenad_pid), 0)
|
||||
# process is running
|
||||
except OSError:
|
||||
assert False, "manage_athenad is dead"
|
||||
|
||||
# def expect_athena_starts(timeout=30):
|
||||
# now = time.time()
|
||||
# athenad_pid = None
|
||||
# while athenad_pid is None:
|
||||
# try:
|
||||
# athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()
|
||||
# return athenad_pid
|
||||
# except subprocess.CalledProcessError:
|
||||
# if time.time() - now > timeout:
|
||||
# assert False, f"Athena did not start within {timeout} seconds"
|
||||
# time.sleep(0.5)
|
||||
def expect_athena_starts(timeout=30):
|
||||
now = time.time()
|
||||
athenad_pid = None
|
||||
while athenad_pid is None:
|
||||
try:
|
||||
athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()
|
||||
return athenad_pid
|
||||
except subprocess.CalledProcessError:
|
||||
if time.time() - now > timeout:
|
||||
assert False, f"Athena did not start within {timeout} seconds"
|
||||
time.sleep(0.5)
|
||||
|
||||
# def athena_post(payload, max_retries=5, wait=5):
|
||||
# tries = 0
|
||||
# while 1:
|
||||
# try:
|
||||
# resp = requests.post(
|
||||
# "https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
|
||||
# headers={
|
||||
# "Authorization": "JWT " + os.getenv("COMMA_JWT"),
|
||||
# "Content-Type": "application/json"
|
||||
# },
|
||||
# data=json.dumps(payload),
|
||||
# timeout=30
|
||||
# )
|
||||
# resp_json = resp.json()
|
||||
# if resp_json.get('error'):
|
||||
# raise Exception(resp_json['error'])
|
||||
# return resp_json
|
||||
# except Exception as e:
|
||||
# time.sleep(wait)
|
||||
# tries += 1
|
||||
# if tries == max_retries:
|
||||
# raise
|
||||
# else:
|
||||
# print(f'athena_post failed {e}. retrying...')
|
||||
def athena_post(payload, max_retries=5, wait=5):
|
||||
tries = 0
|
||||
while 1:
|
||||
try:
|
||||
resp = requests.post(
|
||||
"https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
|
||||
headers={
|
||||
"Authorization": "JWT " + os.getenv("COMMA_JWT"),
|
||||
"Content-Type": "application/json"
|
||||
},
|
||||
data=json.dumps(payload),
|
||||
timeout=30
|
||||
)
|
||||
resp_json = resp.json()
|
||||
if resp_json.get('error'):
|
||||
raise Exception(resp_json['error'])
|
||||
return resp_json
|
||||
except Exception as e:
|
||||
time.sleep(wait)
|
||||
tries += 1
|
||||
if tries == max_retries:
|
||||
raise
|
||||
else:
|
||||
print(f'athena_post failed {e}. retrying...')
|
||||
|
||||
# def expect_athena_registers():
|
||||
# resp = athena_post({
|
||||
# "method": "echo",
|
||||
# "params": ["hello"],
|
||||
# "id": 0,
|
||||
# "jsonrpc": "2.0"
|
||||
# }, max_retries=12, wait=5)
|
||||
# assert resp.get('result') == "hello", f'Athena failed to register ({resp})'
|
||||
def expect_athena_registers():
|
||||
resp = athena_post({
|
||||
"method": "echo",
|
||||
"params": ["hello"],
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
}, max_retries=12, wait=5)
|
||||
assert resp.get('result') == "hello", f'Athena failed to register ({resp})'
|
||||
|
||||
# try:
|
||||
# athenad_pid = expect_athena_starts()
|
||||
# # kill athenad and ensure it is restarted (check_output will throw if it is not)
|
||||
# os.kill(int(athenad_pid), signal.SIGINT)
|
||||
# expect_athena_starts()
|
||||
try:
|
||||
athenad_pid = expect_athena_starts()
|
||||
# kill athenad and ensure it is restarted (check_output will throw if it is not)
|
||||
os.kill(int(athenad_pid), signal.SIGINT)
|
||||
expect_athena_starts()
|
||||
|
||||
# if not os.getenv('COMMA_JWT'):
|
||||
# print('WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai')
|
||||
# return
|
||||
if not os.getenv('COMMA_JWT'):
|
||||
print('WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai')
|
||||
return
|
||||
|
||||
# expect_athena_registers()
|
||||
expect_athena_registers()
|
||||
|
||||
# print("ATHENA: getSimInfo")
|
||||
# resp = athena_post({
|
||||
# "method": "getSimInfo",
|
||||
# "id": 0,
|
||||
# "jsonrpc": "2.0"
|
||||
# })
|
||||
# assert resp.get('result'), resp
|
||||
# assert 'sim_id' in resp['result'], resp['result']
|
||||
print("ATHENA: getSimInfo")
|
||||
resp = athena_post({
|
||||
"method": "getSimInfo",
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
})
|
||||
assert resp.get('result'), resp
|
||||
assert 'sim_id' in resp['result'], resp['result']
|
||||
|
||||
# print("ATHENA: takeSnapshot")
|
||||
# resp = athena_post({
|
||||
# "method": "takeSnapshot",
|
||||
# "id": 0,
|
||||
# "jsonrpc": "2.0"
|
||||
# })
|
||||
# assert resp.get('result'), resp
|
||||
# assert resp['result']['jpegBack'], resp['result']
|
||||
print("ATHENA: takeSnapshot")
|
||||
resp = athena_post({
|
||||
"method": "takeSnapshot",
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
})
|
||||
assert resp.get('result'), resp
|
||||
assert resp['result']['jpegBack'], resp['result']
|
||||
|
||||
# @with_processes(["thermald"])
|
||||
# def test_athena_thermal():
|
||||
# print("ATHENA: getMessage(thermal)")
|
||||
# resp = athena_post({
|
||||
# "method": "getMessage",
|
||||
# "params": {"service": "thermal", "timeout": 5000},
|
||||
# "id": 0,
|
||||
# "jsonrpc": "2.0"
|
||||
# })
|
||||
# assert resp.get('result'), resp
|
||||
# assert resp['result']['thermal'], resp['result']
|
||||
# test_athena_thermal()
|
||||
# finally:
|
||||
# try:
|
||||
# athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()
|
||||
# except subprocess.CalledProcessError:
|
||||
# athenad_pid = None
|
||||
@with_processes(["thermald"])
|
||||
def test_athena_thermal():
|
||||
print("ATHENA: getMessage(thermal)")
|
||||
resp = athena_post({
|
||||
"method": "getMessage",
|
||||
"params": {"service": "thermal", "timeout": 5000},
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
})
|
||||
assert resp.get('result'), resp
|
||||
assert resp['result']['thermal'], resp['result']
|
||||
test_athena_thermal()
|
||||
finally:
|
||||
try:
|
||||
athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()
|
||||
except subprocess.CalledProcessError:
|
||||
athenad_pid = None
|
||||
|
||||
# try:
|
||||
# os.kill(int(manage_athenad_pid), signal.SIGINT)
|
||||
# os.kill(int(athenad_pid), signal.SIGINT)
|
||||
# except (OSError, TypeError):
|
||||
# pass
|
||||
try:
|
||||
os.kill(int(manage_athenad_pid), signal.SIGINT)
|
||||
os.kill(int(athenad_pid), signal.SIGINT)
|
||||
except (OSError, TypeError):
|
||||
pass
|
||||
|
||||
# TODO: re-enable when jenkins test has /data/pythonpath -> /data/openpilot
|
||||
# @phone_only
|
||||
|
|
|
@ -130,7 +130,7 @@ def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition):
|
|||
|
||||
|
||||
def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition):
|
||||
new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 100]))
|
||||
new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80]))
|
||||
|
||||
if not ignition:
|
||||
new_speed = min(30, new_speed)
|
||||
|
@ -192,10 +192,12 @@ def thermald_thread():
|
|||
if health is not None:
|
||||
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
|
||||
|
||||
try:
|
||||
network_type = get_network_type()
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
# get_network_type is an expensive call. update every 10s
|
||||
if (count % int(10. / DT_TRML)) == 0:
|
||||
try:
|
||||
network_type = get_network_type()
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
|
||||
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
|
||||
msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent))
|
||||
|
@ -272,13 +274,16 @@ def thermald_thread():
|
|||
last_update = now
|
||||
dt = now - last_update
|
||||
|
||||
if dt.days > DAYS_NO_CONNECTIVITY_MAX:
|
||||
update_failed_count = params.get("UpdateFailedCount")
|
||||
update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
|
||||
|
||||
if dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
|
||||
if current_connectivity_alert != "expired":
|
||||
current_connectivity_alert = "expired"
|
||||
params.delete("Offroad_ConnectivityNeededPrompt")
|
||||
params.put("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"]))
|
||||
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
|
||||
remaining_time = str(DAYS_NO_CONNECTIVITY_MAX - dt.days)
|
||||
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
|
||||
if current_connectivity_alert != "prompt" + remaining_time:
|
||||
current_connectivity_alert = "prompt" + remaining_time
|
||||
alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"])
|
||||
|
|
|
@ -293,6 +293,7 @@ def attempt_update():
|
|||
|
||||
|
||||
def main(gctx=None):
|
||||
update_failed_count = 0
|
||||
overlay_init_done = False
|
||||
wait_helper = WaitTimeHelper()
|
||||
params = Params()
|
||||
|
@ -312,6 +313,7 @@ def main(gctx=None):
|
|||
raise RuntimeError("couldn't get overlay lock; is another updated running?")
|
||||
|
||||
while True:
|
||||
update_failed_count += 1
|
||||
time_wrong = datetime.datetime.now().year < 2019
|
||||
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
|
||||
|
||||
|
@ -335,6 +337,7 @@ def main(gctx=None):
|
|||
|
||||
if params.get("IsOffroad") == b"1":
|
||||
attempt_update()
|
||||
update_failed_count = 0
|
||||
else:
|
||||
cloudlog.info("not running updater, openpilot running")
|
||||
|
||||
|
@ -348,8 +351,8 @@ def main(gctx=None):
|
|||
overlay_init_done = False
|
||||
except Exception:
|
||||
cloudlog.exception("uncaught updated exception, shouldn't happen")
|
||||
overlay_init_done = False
|
||||
|
||||
params.put("UpdateFailedCount", str(update_failed_count))
|
||||
wait_between_updates(wait_helper.ready_event)
|
||||
if wait_helper.shutdown:
|
||||
break
|
||||
|
|
Loading…
Reference in New Issue