diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 000000000..64a55daf8 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,585 @@ +[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*(# )??$ + +# 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 diff --git a/.travis.yml b/.travis.yml index 3dfe33823..179beaa8e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,8 +7,12 @@ install: - docker build -t tmppilot -f Dockerfile.openpilot . script: + - docker run + tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py' + - docker run + tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda")' + - docker run + tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda"); exit $(($? & 3))' - docker run -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py' - - docker run - tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py' diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index de602f3bc..62c655d7c 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -21,6 +21,7 @@ RUN apt-get update && apt-get install -y \ ocl-icd-opencl-dev \ opencl-headers +RUN pip install --upgrade pip==18.0 RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib==2.1.2 COPY requirements_openpilot.txt /tmp/ @@ -28,12 +29,14 @@ RUN pip install -r /tmp/requirements_openpilot.txt ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH +COPY ./.pylintrc /tmp/openpilot/.pylintrc COPY ./common /tmp/openpilot/common COPY ./cereal /tmp/openpilot/cereal COPY ./opendbc /tmp/openpilot/opendbc COPY ./selfdrive /tmp/openpilot/selfdrive COPY ./phonelibs /tmp/openpilot/phonelibs COPY ./pyextra /tmp/openpilot/pyextra +COPY ./panda /tmp/openpilot/panda RUN mkdir -p /tmp/openpilot/selfdrive/test/out RUN make -C /tmp/openpilot/selfdrive/controls/lib/longitudinal_mpc clean diff --git a/README.md b/README.md index f8052b285..3a69dc1c0 100644 --- a/README.md +++ b/README.md @@ -68,6 +68,7 @@ Supported Cars | Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Chrysler | Pacifica 2018 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | | Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | +| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA | | GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| | Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | Bosch | @@ -76,26 +77,30 @@ Supported Cars | Honda | Civic Hatchback 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch | | Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph1| 12mph | Nidec | | Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch | +| Honda | CR-V Hybrid 2019 | All | Yes | Stock | 0mph | 12mph | Bosch | | Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 0mph | Inverted Nidec | +| Honda | Passport 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | | Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | | Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec | | Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec | | Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6| | Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6| | Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6| -| Jeep | Grand Cherokee 2017-19 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | +| Jeep | Grand Cherokee 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA | +| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA | | Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| -| Lexus | RX Hybrid 2016-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru | | Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota | | Toyota | C-HR 2017-184 | All | Yes | Stock | 0mph | 0mph | Toyota | | Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius Prime 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius Prime 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | diff --git a/RELEASES.md b/RELEASES.md index 48e3b647d..c63b3ef79 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,13 +1,27 @@ +Version 0.5.11 (2019-04-17) +======================== + * Add support for Subaru + * Reduce panda power consumption by 60% when car is off + * Fix controlsd lag every 6 minutes. This would sometimes cause disengagements + * Fix bug in controls with new angle-offset learner in MPC + * Reduce cpu consumption of ubloxd by rewriting it in C++ + * Improve driver monitoring model and face detection + * Improve performance of visiond and ui + * Honda Passport 2019 support + * Lexus RX Hybrid 2019 support thanks to schomems! + * Improve road selection heuristic in mapd + * Add Lane Departure Warning to dashboard for Toyota thanks to arne182 + Version 0.5.10 (2019-03-19) ======================== - * Self-tuning vehicle parameters: steering offset, tires stiffness and steering ratio + * Self-tuning vehicle parameters: steering offset, tire stiffness and steering ratio * Improve longitudinal control at low speed when lead vehicle harshly decelerates * Fix panda bug going unexpectedly in DCP mode when EON is connected * Reduce white panda power consumption by 500mW when EON is disconnected by turning off WIFI * New Driver Monitoring Model * Support QR codes for login using comma connect - * Refactor comma pedal FW and use CRC-8 checksum algorithm for safety. Reflashing pedal is required. - Please see `#hw-pedal` on [discord](discord.comma.ai) for assistance updating comma pedal. + * Refactor comma pedal FW and use CRC-8 checksum algorithm for safety. Reflashing pedal is required. + Please see `#hw-pedal` on [discord](discord.comma.ai) for assistance updating comma pedal. * Additional speed limit rules for Germany thanks to arne182 * Allow negative speed limit offsets diff --git a/SAFETY.md b/SAFETY.md index b5800a0d5..3cda811e0 100644 --- a/SAFETY.md +++ b/SAFETY.md @@ -133,6 +133,19 @@ Chrysler/Jeep/Fiat (Lateral only) units above the actual EPS generated motor torque to ensure limited differences between commanded and actual torques. +Subaru (Lateral only) +------ + + - While the system is engaged, steer commands are subject to the same limits used by + the stock system. + + - Steering torque is controlled through the 0x122 CAN message and it's limited by the panda firmware and by + openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will fault for + commands outside the values of -2047 and 2047. A steering torque rate limit is enforced by the panda firmware and by + openpilot, so that the commanded steering torque must rise from 0 to max value no faster than + 0.41s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's + torque exceeds 60 units in the opposite dicrection to ensure limited applied torque against the + driver's will. **Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or not fully meeting the above requirements. diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 342f832c2..b572a93fa 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/car.capnp b/cereal/car.capnp index 8fb312867..2915d6871 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -247,6 +247,8 @@ struct CarControl { audibleAlert @5: AudibleAlert; rightLaneVisible @6: Bool; leftLaneVisible @7: Bool; + rightLaneDepart @8: Bool; + leftLaneDepart @9: Bool; enum VisualAlert { # these are the choices from the Honda diff --git a/common/dbc.py b/common/dbc.py index 6accad43f..693b2985a 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -7,9 +7,9 @@ from collections import namedtuple, defaultdict def int_or_float(s): # return number, trying to maintain int format - try: - return int(s) - except ValueError: + if s.isdigit(): + return int(s, 10) + else: return float(s) DBCSignal = namedtuple( @@ -21,7 +21,7 @@ class dbc(object): def __init__(self, fn): self.name, _ = os.path.splitext(os.path.basename(fn)) with open(fn) as f: - self.txt = f.read().split("\n") + self.txt = f.readlines() self._warned_addresses = set() # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py @@ -51,7 +51,8 @@ class dbc(object): dat = bo_regexp.match(l) if dat is None: - print "bad BO", l + print("bad BO {0}".format(l)) + name = dat.group(2) size = int(dat.group(3)) ids = int(dat.group(1), 0) # could be hex @@ -67,8 +68,9 @@ class dbc(object): if dat is None: dat = sgm_regexp.match(l) go = 1 + if dat is None: - print "bad SG", l + print("bad SG {0}".format(l)) sgname = dat.group(1) start_bit = int(dat.group(go+2)) @@ -90,12 +92,13 @@ class dbc(object): dat = val_regexp.match(l) if dat is None: - print "bad VAL", l + print("bad VAL {0}".format(l)) + ids = int(dat.group(1), 0) # could be hex sgname = dat.group(2) defvals = dat.group(3) - defvals = defvals.replace("?","\?") #escape sequence in C++ + defvals = defvals.replace("?",r"\?") #escape sequence in C++ defvals = defvals.split('"')[:-1] defs = defvals[1::2] @@ -109,7 +112,7 @@ class dbc(object): self.def_vals[ids].append((sgname, defvals)) - for msg in self.msgs.viewvalues(): + for msg in self.msgs.values(): msg[1].sort(key=lambda x: x.start_bit) self.msg_name_to_address = {} @@ -208,7 +211,7 @@ class dbc(object): name = msg[0][0] if debug: - print name + print(name) st = x[2].ljust(8, '\x00') le, be = None, None @@ -252,7 +255,7 @@ class dbc(object): tmp = tmp * factor + offset # if debug: - # print "%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1]) + # print("%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])) if arr is None: out[s[0]] = tmp diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py index fd6b2bf7b..f41115d1c 100644 --- a/common/ffi_wrapper.py +++ b/common/ffi_wrapper.py @@ -4,10 +4,8 @@ import fcntl import hashlib from cffi import FFI -TMPDIR = "/tmp/ccache" - -def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=None): +def ffi_wrap(name, c_code, c_header, tmpdir="/tmp/ccache", cflags="", libraries=None): if libraries is None: libraries = [] @@ -24,7 +22,7 @@ def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=None): try: mod = __import__(cache) except Exception: - print "cache miss", cache + print("cache miss {0}".format(cache)) compile_code(cache, c_code, c_header, tmpdir, cflags, libraries) mod = __import__(cache) finally: diff --git a/common/fingerprints.py b/common/fingerprints.py index d4845f8d4..5a29c9555 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -61,4 +61,4 @@ def eliminate_incompatible_cars(msg, candidate_cars): def all_known_cars(): """Returns a list of all known car strings.""" - return _FINGERPRINTS.keys() + return list(_FINGERPRINTS.keys()) diff --git a/common/sympy_helpers.py b/common/sympy_helpers.py index 879eb71bb..f20bdb08a 100644 --- a/common/sympy_helpers.py +++ b/common/sympy_helpers.py @@ -75,7 +75,7 @@ def sympy_into_c(sympy_functions): routines.append(r) [(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf") - c_code = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_code.split("\n"))) - c_header = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_header.split("\n"))) + 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] != '#') return c_header, c_code diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 42432a7bb..78c9df989 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,6 +1,7 @@ import numpy as np import common.transformations.orientation as orient import cv2 +import math FULL_FRAME_SIZE = (1164, 874) W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] @@ -12,6 +13,17 @@ eon_intrinsics = np.array([ [ 0., FOCAL, H/2.], [ 0., 0., 1.]]) + +leon_dcam_intrinsics = np.array([ + [650, 0, 816//2], + [ 0, 650, 612//2], + [ 0, 0, 1]]) + +eon_dcam_intrinsics = np.array([ + [860, 0, 1152//2], + [ 0, 860, 864//2], + [ 0, 0, 1]]) + # aka 'K_inv' aka view_frame_from_camera_frame eon_intrinsics_inv = np.linalg.inv(eon_intrinsics) @@ -64,7 +76,7 @@ def normalize(img_pts, intrinsics=eon_intrinsics): input_shape = img_pts.shape img_pts = np.atleast_2d(img_pts) img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1)))) - img_pts_normalized = intrinsics_inv.dot(img_pts.T).T + img_pts_normalized = img_pts.dot(intrinsics_inv.T) img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan return img_pts_normalized[:,:2].reshape(input_shape) @@ -76,7 +88,7 @@ def denormalize(img_pts, intrinsics=eon_intrinsics): input_shape = img_pts.shape img_pts = np.atleast_2d(img_pts) img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1)))) - img_pts_denormalized = intrinsics.dot(img_pts.T).T + img_pts_denormalized = img_pts.dot(intrinsics.T) img_pts_denormalized[img_pts_denormalized[:,0] > W] = np.nan img_pts_denormalized[img_pts_denormalized[:,0] < 0] = np.nan img_pts_denormalized[img_pts_denormalized[:,1] > H] = np.nan @@ -147,28 +159,44 @@ def transform_img(base_img, from_intr=eon_intrinsics, to_intr=eon_intrinsics, calib_rot_view=None, - output_size=None): - cy = from_intr[1,2] + output_size=None, + pretransform=None, + top_hacks=True): size = base_img.shape[:2] if not output_size: output_size = size[::-1] - h = 1.22 - quadrangle = np.array([[0, cy + 20], - [size[1]-1, cy + 20], - [0, size[0]-1], - [size[1]-1, size[0]-1]], dtype=np.float32) - quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1)))) - quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1], - h*np.ones(4), - h/quadrangle_norm[:,1])) - rot = orient.rot_from_euler(augment_eulers) - if calib_rot_view is not None: - rot = calib_rot_view.dot(rot) - to_extrinsics = np.hstack((rot.T, -augment_trans[:,None])) - to_KE = to_intr.dot(to_extrinsics) - warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1))))) - warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2], - warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32) - M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32)) + + cy = from_intr[1,2] + def get_M(h=1.22): + quadrangle = np.array([[0, cy + 20], + [size[1]-1, cy + 20], + [0, size[0]-1], + [size[1]-1, size[0]-1]], dtype=np.float32) + quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1)))) + quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1], + h*np.ones(4), + h/quadrangle_norm[:,1])) + rot = orient.rot_from_euler(augment_eulers) + if calib_rot_view is not None: + rot = calib_rot_view.dot(rot) + to_extrinsics = np.hstack((rot.T, -augment_trans[:,None])) + to_KE = to_intr.dot(to_extrinsics) + warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1))))) + warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2], + warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32) + M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32)) + return M + + M = get_M() + if pretransform is not None: + M = M.dot(pretransform) augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE) + + if top_hacks: + cyy = int(math.ceil(to_intr[1,2])) + M = get_M(1000) + if pretransform is not None: + M = M.dot(pretransform) + augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE) + return augmented_rgb diff --git a/common/transformations/model.py b/common/transformations/model.py index 2d41d9ceb..afb4d6671 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,9 +1,9 @@ import numpy as np from common.transformations.camera import eon_focal_length, \ - vp_from_ke, \ - get_view_frame_from_road_frame, \ - FULL_FRAME_SIZE + vp_from_ke, \ + get_view_frame_from_road_frame, \ + FULL_FRAME_SIZE # segnet @@ -32,7 +32,7 @@ model_intrinsics = np.array( # MED model -MEDMODEL_INPUT_SIZE = (640, 240) +MEDMODEL_INPUT_SIZE = (512, 256) MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) MEDMODEL_CY = 47.6 diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 41f8a607c..4b7c4c036 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -19,6 +19,8 @@ function launch { echo 0-3 > /dev/cpuset/foreground/cpus echo 0-3 > /dev/cpuset/android/cpus + # handle pythonpath + ln -s /data/openpilot /data/pythonpath export PYTHONPATH="$PWD" # start manager diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc index 0b9f8060d..b860a1185 100644 Binary files a/models/monitoring_model.dlc and b/models/monitoring_model.dlc differ diff --git a/opendbc/ford_cgea1_2_bodycan_2011.dbc b/opendbc/ford_cgea1_2_bodycan_2011.dbc index e13743a66..49fcae19c 100644 --- a/opendbc/ford_cgea1_2_bodycan_2011.dbc +++ b/opendbc/ford_cgea1_2_bodycan_2011.dbc @@ -31,6 +31,8 @@ NS_ : BU_BO_REL_ SG_MUL_VAL_ +BS_: + BU_: XXX diff --git a/opendbc/ford_cgea1_2_ptcan_2011.dbc b/opendbc/ford_cgea1_2_ptcan_2011.dbc index cc5635fa8..128721cbb 100644 --- a/opendbc/ford_cgea1_2_ptcan_2011.dbc +++ b/opendbc/ford_cgea1_2_ptcan_2011.dbc @@ -31,6 +31,8 @@ NS_ : BU_BO_REL_ SG_MUL_VAL_ +BS_: + BU_: XXX diff --git a/opendbc/ford_fusion_2018_adas.dbc b/opendbc/ford_fusion_2018_adas.dbc index 63a0b9909..2e1647b3d 100644 --- a/opendbc/ford_fusion_2018_adas.dbc +++ b/opendbc/ford_fusion_2018_adas.dbc @@ -31,6 +31,8 @@ NS_ : BU_BO_REL_ SG_MUL_VAL_ +BS_: + BU_: XXX BO_ 1280 Object_00: 8 XXX diff --git a/opendbc/ford_fusion_2018_pt.dbc b/opendbc/ford_fusion_2018_pt.dbc index 1667a360b..c4b706d06 100644 --- a/opendbc/ford_fusion_2018_pt.dbc +++ b/opendbc/ford_fusion_2018_pt.dbc @@ -31,6 +31,8 @@ NS_ : BU_BO_REL_ SG_MUL_VAL_ +BS_: + BU_: XXX BO_ 130 EPAS_INFO: 8 XXX @@ -128,9 +130,10 @@ BO_ 984 Lane_Keep_Assist_Ui: 8 XXX SG_ Hands_Warning : 49|1@0+ (1,0) [0|1] "" XXX SG_ Set_Me_X30 : 63|8@0+ (1,0) [0|255] "" XXX +CM_ SG_ 970 Lkas_Action "only vals 4, 5, 8, 9 seem to work. 4 and 5 are a bit smoother" ; + VAL_ 357 Cruise_State 4 "active" 3 "standby" 0 "off" ; VAL_ 970 Lkas_Action 15 "off" 9 "abrupt" 8 "abrupt2" 5 "smooth" 4 "smooth2" ; VAL_ 970 Lkas_Alert 15 "no_alert" 3 "high_intensity" 2 "mid_intensity" 1 "low_intensity" ; VAL_ 972 LaActAvail_D_Actl 3 "available" 2 "tbd" 1 "not_available" 0 "fault" ; VAL_ 984 Lines_Hud 15 "none" 11 "grey_yellow" 8 "green_red" 7 "yellow_grey" 6 "grey_grey" 4 "red_green" 3 "green_green" ; -CM_ SG_ 970 Lkas_Action "only vals 4, 5, 8, 9 seem to work. 4 and 5 are a bit smoother" ; diff --git a/opendbc/generator/honda/honda_civic_touring_2016_can.dbc b/opendbc/generator/honda/honda_civic_touring_2016_can.dbc index 8af78ca05..3207c7b97 100644 --- a/opendbc/generator/honda/honda_civic_touring_2016_can.dbc +++ b/opendbc/generator/honda/honda_civic_touring_2016_can.dbc @@ -50,6 +50,9 @@ BO_ 450 EPB_STATUS: 8 EPB SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 493 HUD_SETTING: 8 XXX + SG_ SPEED_UNIT : 5|1@0+ (1,0) [0|1] "" EON + BO_ 487 BRAKE_PRESSURE: 4 VSA SG_ BRAKE_PRESSURE1 : 7|10@0+ (0.015625,-103) [0|1000] "" EON SG_ BRAKE_PRESSURE2 : 9|10@0+ (0.015625,-103) [0|1000] "" EON @@ -125,6 +128,7 @@ VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_spe VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P" ; VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ; VAL_ 450 EPB_STATE 3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged" ; +VAL_ 493 SPEED_UNIT 1 "mph" 0 "kph" ; VAL_ 545 ECON_ON_2 0 "off" 3 "on" ; VAL_ 662 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ; VAL_ 662 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ; diff --git a/opendbc/generator/toyota/_toyota_2017.dbc b/opendbc/generator/toyota/_toyota_2017.dbc index c0e3a8648..0e2a242b1 100644 --- a/opendbc/generator/toyota/_toyota_2017.dbc +++ b/opendbc/generator/toyota/_toyota_2017.dbc @@ -254,9 +254,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc b/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc index caf7b6388..724b6e834 100644 --- a/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc +++ b/opendbc/generator/toyota/toyota_corolla_2017_pt.dbc @@ -23,6 +23,7 @@ BO_ 610 EPS_STATUS: 5 EPS BO_ 956 GEAR_PACKET: 8 XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ SPORT_ON : 3|1@0+ (1,0) [0|1] "" XXX CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc index 7cee7c73d..675e56f23 100644 --- a/opendbc/honda_civic_touring_2016_can_generated.dbc +++ b/opendbc/honda_civic_touring_2016_can_generated.dbc @@ -288,6 +288,9 @@ BO_ 450 EPB_STATUS: 8 EPB SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON +BO_ 493 HUD_SETTING: 8 XXX + SG_ SPEED_UNIT : 5|1@0+ (1,0) [0|1] "" EON + BO_ 487 BRAKE_PRESSURE: 4 VSA SG_ BRAKE_PRESSURE1 : 7|10@0+ (0.015625,-103) [0|1000] "" EON SG_ BRAKE_PRESSURE2 : 9|10@0+ (0.015625,-103) [0|1000] "" EON @@ -363,6 +366,7 @@ VAL_ 399 STEER_STATUS 6 "tmp_fault" 5 "fault_1" 4 "no_torque_alert_2" 3 "low_spe VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P" ; VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ; VAL_ 450 EPB_STATE 3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged" ; +VAL_ 493 SPEED_UNIT 1 "mph" 0 "kph" ; VAL_ 545 ECON_ON_2 0 "off" 3 "on" ; VAL_ 662 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ; VAL_ 662 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ; diff --git a/opendbc/honda_clarity_hybrid_2018_can.dbc b/opendbc/honda_clarity_hybrid_2018_can.dbc index 86272de0b..3d46f52cd 100644 --- a/opendbc/honda_clarity_hybrid_2018_can.dbc +++ b/opendbc/honda_clarity_hybrid_2018_can.dbc @@ -409,6 +409,7 @@ BO_TX_BU_ 862 : NEO,ADAS; BO_TX_BU_ 927 : NEO,ADAS; +CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; CM_ SG_ 401 GEAR "10 = reverse, 11 = transition"; CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping"; CM_ SG_ 773 PASS_AIRBAG_ON "Might just be indicator light"; @@ -430,4 +431,3 @@ VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ; VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ; VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ; VAL_ 927 ACC_ALERTS 29 "esp_active_acc_canceled" 10 "b_pedal_applied" 9 "speed_too_low" 8 "speed_too_high" 7 "p_brake_applied" 6 "gear_no_d" 5 "seatbelt" 4 "too_steep_downhill" 3 "too_steep_uphill" 2 "too_close" 1 "no_vehicle_ahead" ; -CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; diff --git a/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc b/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc index 91859aa24..5126bb16a 100644 --- a/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc +++ b/opendbc/honda_odyssey_extreme_edition_2018_china_can.dbc @@ -276,6 +276,7 @@ BO_ 1302 ODOMETER: 8 XXX +CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping"; CM_ SG_ 773 PASS_AIRBAG_OFF "Might just be indicator light"; CM_ SG_ 773 PASS_AIRBAG_ON "Might just be indicator light"; @@ -295,4 +296,3 @@ VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ; VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ; VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ; VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ; -CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc index a207b26d6..69e20ac67 100644 --- a/opendbc/lexus_is_2018_pt_generated.dbc +++ b/opendbc/lexus_is_2018_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/lexus_is_hybrid_2017_pt_generated.dbc b/opendbc/lexus_is_hybrid_2017_pt_generated.dbc index e94824670..75639c994 100644 --- a/opendbc/lexus_is_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_is_hybrid_2017_pt_generated.dbc @@ -15,16 +15,18 @@ BO_ 359 STEERING_IPAS_COMMA: 8 IPAS CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant"; BO_ 512 GAS_COMMAND: 6 EON - SG_ GAS_COMMAND : 7|16@0+ (0.0244140625,0) [0|1] "" INTERCEPTOR - SG_ GAS_COMMAND2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR - SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" INTERCEPTOR + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR BO_ 513 GAS_SENSOR: 6 INTERCEPTOR - SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0244140625,0) [0|1] "" EON - SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.0244140625,-11.962890625) [0|1] "" EON - SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON - SG_ CHECKSUM : 47|8@0+ (1,0) [0|3] "" EON + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON + SG_ STATE : 39|4@0+ (1,0) [0|15] "" EON + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" EON + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" EON VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; @@ -76,7 +78,7 @@ BO_ 37 STEER_ANGLE_SENSOR: 8 XXX SG_ STEER_ANGLE : 3|12@0- (1.5,0) [-500|500] "deg" XXX SG_ STEER_FRACTION : 39|4@0- (0.1,0) [-0.7|0.7] "deg" XXX SG_ STEER_RATE : 35|12@0- (1,0) [-2000|2000] "deg/s" XXX - + BO_ 166 BRAKE: 8 XXX SG_ BRAKE_AMOUNT : 7|8@0+ (1,0) [0|255] "" XXX SG_ BRAKE_PEDAL : 23|8@0+ (1,0) [0|255] "" XXX @@ -92,6 +94,9 @@ BO_ 180 SPEED: 8 XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX SG_ SPEED : 47|16@0+ (0.01,0) [0|250] "kph" XXX +BO_ 353 DSU_SPEED: 8 XXX + SG_ FORWARD_SPEED : 15|16@0- (0.00390625,-30) [0|255] "kph" XXX + BO_ 466 PCM_CRUISE: 8 XXX SG_ GAS_RELEASED : 4|1@0+ (1,0) [0|1] "" XXX SG_ CRUISE_ACTIVE : 5|1@0+ (1,0) [0|1] "" XXX @@ -140,7 +145,10 @@ BO_ 742 LEAD_INFO: 8 DSU BO_ 835 ACC_CONTROL: 8 DSU SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU - SG_ SET_ME_X63 : 23|8@0+ (1,0) [0|255] "" HCU + SG_ SET_ME_X01 : 23|2@0+ (1,0) [0|3] "" HCU + SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX + SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX + SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX SG_ SET_ME_1 : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU @@ -180,7 +188,7 @@ BO_ 1042 LKAS_HUD: 8 XXX BO_ 1553 UI_SEETING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX - + BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -224,7 +232,7 @@ BO_ 1162 RSA2: 8 FCM SG_ SPDUNT : 63|2@0+ (1,0) [0|0] "" XXX SG_ TSRWMSG : 61|2@0+ (1,0) [0|0] "" XXX SG_ SYNCID2 : 59|4@0+ (1,0) [0|0] "" XXX - + BO_ 1163 RSA3: 8 FCM SG_ TSREQPD : 7|1@0+ (1,0) [0|0] "" XXX SG_ TSRMSW : 6|1@0+ (1,0) [0|0] "" XXX @@ -274,15 +282,15 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none"; VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none"; -VAL_ 1042 RIGHT_LINE 3 "orange" 2 "double" 1 "solid" 0 "none"; -VAL_ 1042 LEFT_LINE 3 "orange" 2 "double" 1 "solid" 0 "none"; +VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none"; +VAL_ 1042 LEFT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none"; VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc index acbd9bb19..26e0dd6a4 100644 --- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/mazda_cx5_gt_2017.dbc b/opendbc/mazda_cx5_gt_2017.dbc index d0b84c22a..3926e2dfb 100644 --- a/opendbc/mazda_cx5_gt_2017.dbc +++ b/opendbc/mazda_cx5_gt_2017.dbc @@ -247,7 +247,7 @@ BO_ 1277 NEW_MSG_10: 8 XXX SG_ NEW_SIGNAL_2 : 15|8@0+ (1,0) [0|255] "" XXX SG_ counter : 7|8@0+ (1,0) [0|255] "" XXX -BO_ 1275 2017_5: 8 XXX +BO_ 1275 MSG_2017_5: 8 XXX SG_ counter : 4|5@0+ (1,0) [0|255] "" XXX BO_ 1274 NEW_MSG_12: 8 XXX @@ -565,23 +565,23 @@ BO_ 121 EPB: 8 XXX SG_ NEW_SIGNAL_11 : 45|1@0+ (1,0) [0|1] "" XXX SG_ EPB_ACTIVE : 29|1@0+ (1,0) [0|15] "" XXX -BO_ 1070 2017_1: 8 XXX +BO_ 1070 MSG_2017_1: 8 XXX -BO_ 1183 2017_2: 8 XXX +BO_ 1183 MSG_2017_2: 8 XXX -BO_ 1243 2017_3: 8 XXX +BO_ 1243 MSG_2017_3: 8 XXX SG_ NEW_SIGNAL_1 : 7|64@0+ (1,0) [0|18446744073709552000] "" XXX BO_ 1269 MSG_2017_4: 8 XXX SG_ NEW_SIGNAL_1 : 55|16@0+ (1,0) [0|18446744073709552000] "" XXX -BO_ 1178 2017_6: 8 XXX +BO_ 1178 MSG_2017_6: 8 XXX SG_ NEW_SIGNAL_1 : 7|64@0+ (1,0) [0|18446744073709552000] "" XXX -BO_ 1179 2017_7: 8 XXX +BO_ 1179 MSG_2017_7: 8 XXX SG_ NEW_SIGNAL_1 : 7|64@0+ (1,0) [0|18446744073709552000] "" XXX -BO_ 1435 2017_8: 8 XXX +BO_ 1435 MSG_2017_8: 8 XXX BO_ 253 NEW_MSG_7: 8 XXX SG_ NEW_SIGNAL_1 : 16|1@0+ (1,0) [0|65535] "" XXX diff --git a/opendbc/subaru_global_2017.dbc b/opendbc/subaru_global_2017.dbc index 1c8b796ab..4d0f67515 100644 --- a/opendbc/subaru_global_2017.dbc +++ b/opendbc/subaru_global_2017.dbc @@ -48,7 +48,7 @@ BO_ 64 Throttle: 8 XXX SG_ NEW_SIGNAL_3 : 56|4@1+ (1,0) [0|255] "" XXX SG_ Engine_RPM : 16|12@1+ (1,0) [0|255] "" XXX SG_ Throttle_Cruise : 40|8@1+ (1,0) [0|255] "" XXX - SG_ Throttle_Combo : 55|8@1+ (0.392157,0) [0|255] "" XXX + SG_ Throttle_Combo : 55|8@1+ (1,0) [0|255] "" XXX SG_ Throttle_Pedal : 32|8@1+ (1,0) [0|255] "" XXX @@ -70,12 +70,14 @@ BO_ 72 NEW_MSG_2: 8 XXX BO_ 316 NEW_MSG_3: 8 XXX -BO_ 326 NEW_MSG_4: 8 XXX +BO_ 326 Cruise_Buttons: 8 XXX SG_ Checksum : 0|8@1+ (1,0) [0|255] "" XXX SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX - SG_ NEW_SIGNAL_1 : 16|12@1+ (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_2 : 32|8@1+ (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_3 : 40|2@1+ (1,0) [0|255] "" XXX + SG_ Signal1 : 12|30@1+ (1,0) [0|1073741823] "" XXX + SG_ Main : 42|1@1+ (1,0) [0|1] "" XXX + SG_ set : 43|1@1+ (1,0) [0|1] "" XXX + SG_ Resume : 44|1@1+ (1,0) [0|1] "" XXX + SG_ Signal2 : 45|19@1+ (1,0) [0|524287] "" XXX BO_ 315 G_Sensor: 8 XXX SG_ longitudinal : 63|8@0- (1,0) [0|255] "" XXX @@ -140,19 +142,10 @@ BO_ 544 ES_Brake: 8 XXX BO_ 545 ES_Distance: 8 XXX SG_ Checksum : 0|8@1+ (1,0) [0|255] "" XXX SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX - SG_ Close_Distance : 40|8@1+ (1,0) [0|255] "" XXX - SG_ Distance_Swap : 37|1@1+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_5 : 39|1@1+ (1,0) [0|31] "" XXX - SG_ NEW_SIGNAL_9 : 38|1@1+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_4 : 34|1@1+ (1,0) [0|3] "" XXX - SG_ Brake_Pedal : 33|1@1+ (1,0) [0|255] "" XXX - SG_ NEW_SIGNAL_10 : 35|1@1+ (1,0) [0|3] "" XXX - SG_ Throttle : 16|12@1+ (1,0) [0|65535] "" XXX - SG_ NEW_SIGNAL_1 : 31|4@0+ (1,0) [0|15] "" XXX - SG_ SET_1 : 12|1@0+ (1,0) [0|3] "" XXX - SG_ ACC_ENABLE_BTN : 56|1@0+ (1,0) [0|1] "" XXX - SG_ Car_Follow : 32|1@1+ (1,0) [0|3] "" XXX - SG_ Brake : 36|1@0+ (1,0) [0|3] "" XXX + SG_ Signal1 : 12|20@1+ (1,0) [0|15] "" XXX + SG_ Signal2 : 32|24@1+ (1,0) [0|15] "" XXX + SG_ Main : 56|1@1+ (1,0) [0|1] "" XXX + SG_ Signal3 : 57|7@1+ (1,0) [0|1] "" XXX BO_ 546 ES_Status: 8 XXX SG_ Checksum : 0|8@1+ (1,0) [0|255] "" XXX @@ -225,19 +218,20 @@ BO_ 802 ES_LKAS_State: 8 XXX SG_ Counter : 8|4@1+ (1,0) [0|15] "" XXX SG_ Keep_Hands_On_Wheel : 12|1@1+ (1,0) [0|1] "" XXX SG_ Empty_Box : 13|1@1+ (1,0) [0|1] "" XXX + SG_ Signal1 : 14|3@1+ (1,0) [0|7] "" XXX SG_ LKAS_ACTIVE : 17|1@1+ (1,0) [0|3] "" XXX + SG_ Signal2 : 18|5@1+ (1,0) [0|7] "" XXX SG_ Backward_Speed_Limit_Menu : 23|1@1+ (1,0) [0|1] "" XXX SG_ LKAS_ENABLE_3 : 24|1@1+ (1,0) [0|1] "" XXX + SG_ Signal3 : 25|1@1+ (1,0) [0|1] "" XXX SG_ LKAS_ENABLE_2 : 26|1@1+ (1,0) [0|1] "" XXX - SG_ NEW_SIGNAL_7 : 28|1@1+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_2 : 30|1@1+ (1,0) [0|3] "" XXX + SG_ Signal4 : 27|5@1+ (1,0) [0|1] "" XXX SG_ FCW_Cont_Beep : 32|1@1+ (1,0) [0|1] "" XXX SG_ FCW_Repeated_Beep : 33|1@1+ (1,0) [0|1] "" XXX SG_ Throttle_Management_Activated : 34|1@1+ (1,0) [0|1] "" XXX SG_ Traffic_light_Ahead : 35|1@1+ (1,0) [0|1] "" XXX SG_ Right_Depart : 36|1@1+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_4 : 44|1@1+ (1,0) [0|3] "" XXX - SG_ NEW_SIGNAL_5 : 56|2@1+ (1,0) [0|3] "" XXX + SG_ Signal5 : 37|27@1+ (1,0) [0|1] "" XXX BO_ 805 ES_NEW_MSG_22: 8 XXX SG_ Checksum : 0|8@1+ (1,0) [0|255] "" XXX diff --git a/opendbc/subaru_outback_2015_eyesight.dbc b/opendbc/subaru_outback_2015_eyesight.dbc index aad63e310..f233d3bb3 100644 --- a/opendbc/subaru_outback_2015_eyesight.dbc +++ b/opendbc/subaru_outback_2015_eyesight.dbc @@ -33,7 +33,7 @@ NS_ : BS_: -BU_: XXX 0 +BU_: XXX BO_ 2 Steering: 8 XXX @@ -211,24 +211,24 @@ BO_ 358 ES_Status: 8 XXX SG_ NEW_SIGNAL_5_Blank : 33|1@1+ (1,0) [0|3] "" XXX SG_ NEW_SIGNAL_4_Blank : 34|1@1+ (1,0) [0|7] "" XXX SG_ NEW_SIGNAL_1 : 35|1@0+ (1,0) [0|31] "" XXX - SG_ 3SecondDisengage : 13|2@0+ (1,0) [0|3] "" XXX + SG_ Sig3SecondDisengage : 13|2@0+ (1,0) [0|3] "" XXX SG_ Not_Ready_Startup : 0|3@1+ (1,0) [0|7] "" XXX SG_ Steep_Hill_Disengage : 44|1@0+ (1,0) [0|3] "" XXX SG_ Disengage_Alert : 15|2@0+ (1,0) [0|3] "" XXX BO_ 359 ES_LDW: 8 XXX - SG_ 2Right_Depart : 50|1@1+ (1,0) [0|7] "" XXX - SG_ 2All_Depart : 28|1@0+ (1,0) [0|3] "" XXX - SG_ 3All_Depart : 52|1@0+ (1,0) [0|3] "" XXX + SG_ Sig2Right_Depart : 50|1@1+ (1,0) [0|7] "" XXX + SG_ Sig2All_Depart : 28|1@0+ (1,0) [0|3] "" XXX + SG_ Sig3All_Depart : 52|1@0+ (1,0) [0|3] "" XXX SG_ Left_Depart_Front : 51|1@0+ (1,0) [0|3] "" XXX SG_ All_depart_2015 : 0|1@1+ (1,0) [0|255] "" XXX SG_ LKAS_Inactive_2017 : 36|1@1+ (1,0) [0|3] "" XXX SG_ LKAS_Steer_Active_2017 : 37|1@0+ (1,0) [0|3] "" XXX SG_ Right_Line_2017 : 24|1@1+ (1,0) [0|7] "" XXX SG_ Left_Line_2017 : 25|1@1+ (1,0) [0|3] "" XXX - SG_ 1All_Depart : 31|1@0+ (1,0) [0|15] "" XXX - SG_ 1Right_Depart_Front : 49|1@1+ (1,0) [0|3] "" XXX - SG_ 1Right_Depart : 48|1@1+ (1,0) [0|3] "" XXX + SG_ Sig1All_Depart : 31|1@0+ (1,0) [0|15] "" XXX + SG_ Sig1Right_Depart_Front : 49|1@1+ (1,0) [0|3] "" XXX + SG_ Sig1Right_Depart : 48|1@1+ (1,0) [0|3] "" XXX BO_ 392 Counter_0: 8 XXX SG_ Counter : 16|4@1+ (1,0) [0|15] "" XXX @@ -350,15 +350,15 @@ CM_ SG_ 353 Brake_On "long activatedish"; CM_ SG_ 354 RPM "20hz version of Transmission_Engine under Transmission"; CM_ SG_ 358 Cruise_Activated "is 1 when cruise is able to go"; CM_ SG_ 358 Car_Follow "front car detected"; -CM_ SG_ 358 3SecondDisengage "seatbelt disengage"; +CM_ SG_ 358 Sig3SecondDisengage "seatbelt disengage"; CM_ SG_ 358 Disengage_Alert "seatbelt and steep hill disengage"; -CM_ SG_ 359 2All_Depart "Left and right depart"; +CM_ SG_ 359 Sig2All_Depart "Left and right depart"; CM_ SG_ 359 Left_Depart_Front "warning after acceleration into car in front and left depart"; CM_ SG_ 359 All_depart_2015 "always 1 on 2017"; CM_ SG_ 359 LKAS_Inactive_2017 "1 when not steering, 0 when lkas steering"; -CM_ SG_ 359 1All_Depart "Left and right depart"; -CM_ SG_ 359 1Right_Depart_Front "object in front, right depart, hill steep and seatbelt disengage alert "; -CM_ SG_ 359 1Right_Depart "right depart, hill steep and seatbelt disengage"; +CM_ SG_ 359 Sig1All_Depart "Left and right depart"; +CM_ SG_ 359 Sig1Right_Depart_Front "object in front, right depart, hill steep and seatbelt disengage alert "; +CM_ SG_ 359 Sig1Right_Depart "right depart, hill steep and seatbelt disengage"; CM_ SG_ 642 Counter "Affected by signals"; CM_ SG_ 642 RIGHT_BLINKER "0 off, 2 right, 1 left"; CM_ SG_ 880 Steering_Voltage_Flat "receives later than 371"; diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc index 37d839e44..c257fe2e0 100644 --- a/opendbc/tesla_can.dbc +++ b/opendbc/tesla_can.dbc @@ -307,6 +307,8 @@ BO_ 840 GTW_status: 8 GTW SG_ GTW_statusChecksum : 63|8@0+ (1,0) [0|255] "" NEO SG_ GTW_statusCounter : 51|4@0+ (1,0) [0|15] "" NEO +CM_ "CHFFR_METRIC 1160 DAS_steeringAngleRequest STEER_ANGLE 0.1098666 180; CHFFR_METRIC 264 DI_motorRPM ENGINE_RPM 1 0"; + VAL_ 3 StW_Angl 16383 "SNA" ; VAL_ 3 StW_AnglSens_Id 2 "MUST" 0 "PSBL" 1 "SELF" ; VAL_ 3 StW_AnglSens_Stat 2 "ERR" 3 "ERR_INI" 1 "INI" 0 "OK" ; @@ -417,5 +419,3 @@ VAL_ 904 MCU_clusterReadyForDrive 0 "NO_SNA" 1 "YES" ; VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ; VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ; VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ; - -CM_ "CHFFR_METRIC 1160 DAS_steeringAngleRequest STEER_ANGLE 0.1098666 180; CHFFR_METRIC 264 DI_motorRPM ENGINE_RPM 1 0"; diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc index 64c39ff82..7e4742ea9 100644 --- a/opendbc/toyota_avalon_2017_pt_generated.dbc +++ b/opendbc/toyota_avalon_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc index 5d4fd39a6..066b41767 100644 --- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_chr_2018_pt_generated.dbc b/opendbc/toyota_chr_2018_pt_generated.dbc index a2e169f94..16412c7bc 100644 --- a/opendbc/toyota_chr_2018_pt_generated.dbc +++ b/opendbc/toyota_chr_2018_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc b/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc index 574d3c200..961843924 100644 --- a/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_chr_hybrid_2018_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc index 14aff972e..866711e37 100644 --- a/opendbc/toyota_corolla_2017_pt_generated.dbc +++ b/opendbc/toyota_corolla_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; @@ -321,6 +321,7 @@ BO_ 610 EPS_STATUS: 5 EPS BO_ 956 GEAR_PACKET: 8 XXX SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX + SG_ SPORT_ON : 3|1@0+ (1,0) [0|1] "" XXX CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 548 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc index cd78b63de..64f920b4b 100644 --- a/opendbc/toyota_highlander_2017_pt_generated.dbc +++ b/opendbc/toyota_highlander_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc index 08831635d..b9a1febab 100644 --- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_prius_2010_pt.dbc b/opendbc/toyota_prius_2010_pt.dbc index 5dba51958..634b1dc0b 100644 --- a/opendbc/toyota_prius_2010_pt.dbc +++ b/opendbc/toyota_prius_2010_pt.dbc @@ -169,6 +169,7 @@ BO_ 452 POWERTRAIN: 8 XXX +CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; CM_ SG_ 36 ACCEL_Y "unit is tbd"; CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 36 YAW_RATE "verify"; @@ -200,4 +201,3 @@ VAL_ 1042 RIGHT_LINE 3 "orange" 2 "double" 1 "solid" 0 "none" ; VAL_ 1042 LEFT_LINE 3 "orange" 2 "double" 1 "solid" 0 "none" ; VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none" ; VAL_ 1553 UNITS 1 "km" 2 "miles" ; -CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc index abc4f0817..23410f8e2 100644 --- a/opendbc/toyota_prius_2017_pt_generated.dbc +++ b/opendbc/toyota_prius_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc index 872715625..89ad1378e 100644 --- a/opendbc/toyota_rav4_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc index a75dcb3af..2d3e3bcc7 100644 --- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc index 32ef3f518..256a702ab 100644 --- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc +++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc @@ -288,9 +288,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; -VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; +VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/panda/.dockerignore b/panda/.dockerignore new file mode 100644 index 000000000..f04ae5c0a --- /dev/null +++ b/panda/.dockerignore @@ -0,0 +1,3 @@ +.git +.DS_Store +boardesp/esp-open-sdk diff --git a/panda/Dockerfile b/panda/Dockerfile new file mode 100644 index 000000000..100509f51 --- /dev/null +++ b/panda/Dockerfile @@ -0,0 +1,64 @@ +FROM ubuntu:16.04 +ENV PYTHONUNBUFFERED 1 + +RUN apt-get update && apt-get install -y \ + autoconf \ + automake \ + bash \ + bison \ + bzip2 \ + curl \ + dfu-util \ + flex \ + g++ \ + gawk \ + gcc \ + git \ + gperf \ + help2man \ + iputils-ping \ + libexpat-dev \ + libstdc++-arm-none-eabi-newlib \ + libtool \ + libtool-bin \ + libusb-1.0-0 \ + make \ + ncurses-dev \ + network-manager \ + python-dev \ + python-serial \ + sed \ + texinfo \ + unrar-free \ + unzip \ + wget \ + build-essential \ + python-dev \ + python-pip \ + screen \ + vim \ + wget \ + wireless-tools + +RUN pip install --upgrade pip==18.0 + +COPY requirements.txt /tmp/ +RUN pip install -r /tmp/requirements.txt + +RUN mkdir -p /home/batman +ENV HOME /home/batman + +ENV PYTHONPATH /tmp:$PYTHONPATH + +COPY ./boardesp/get_sdk_ci.sh /tmp/panda/boardesp/ + +RUN useradd --system -s /sbin/nologin pandauser +RUN mkdir -p /tmp/panda/boardesp/esp-open-sdk +RUN chown pandauser /tmp/panda/boardesp/esp-open-sdk +USER pandauser +RUN cd /tmp/panda/boardesp && ./get_sdk_ci.sh +USER root + +COPY ./xx/pandaextra /tmp/pandaextra + +ADD ./panda.tar.gz /tmp/panda diff --git a/panda/Jenkinsfile b/panda/Jenkinsfile new file mode 100644 index 000000000..b90a8ca38 --- /dev/null +++ b/panda/Jenkinsfile @@ -0,0 +1,55 @@ +pipeline { + agent any + environment { + AUTHOR = """${sh( + returnStdout: true, + script: "git --no-pager show -s --format='%an' ${GIT_COMMIT}" + ).trim()}""" + + DOCKER_IMAGE_TAG = "panda:build-${env.BUILD_ID}" + DOCKER_NAME = "panda-test-${env.BUILD_ID}" + } + stages { + stage('Build Docker Image') { + steps { + timeout(time: 60, unit: 'MINUTES') { + script { + sh 'git clone --no-checkout --depth 1 git@github.com:commaai/xx.git || true' + sh 'cd xx && git fetch origin && git checkout origin/master -- pandaextra && cd ..' // Needed for certs for panda flashing + sh 'git archive -v -o panda.tar.gz --format=tar.gz HEAD' + dockerImage = docker.build("${env.DOCKER_IMAGE_TAG}") + } + } + } + } + stage('Test Dev Build') { + steps { + lock(resource: "Pandas", inversePrecedence: true, quantity:1){ + timeout(time: 60, unit: 'MINUTES') { + sh "docker run --name ${env.DOCKER_NAME} --privileged --volume /dev/bus/usb:/dev/bus/usb --volume /var/run/dbus:/var/run/dbus --net host ${env.DOCKER_IMAGE_TAG} bash -c 'cd /tmp/panda; ./run_automated_tests.sh '" + } + } + } + } + stage('Test EON Build') { + steps { + lock(resource: "Pandas", inversePrecedence: true, quantity:1){ + timeout(time: 60, unit: 'MINUTES') { + sh "docker cp ${env.DOCKER_NAME}:/tmp/panda/nosetests.xml test_results_dev.xml" + sh "touch EON && docker cp EON ${env.DOCKER_NAME}:/EON" + sh "docker start -a ${env.DOCKER_NAME}" + } + } + } + } + } + post { + always { + script { + sh "docker cp ${env.DOCKER_NAME}:/tmp/panda/nosetests.xml test_results_EON.xml" + sh "docker rm ${env.DOCKER_NAME}" + } + junit "test_results*.xml" + } + } +} \ No newline at end of file diff --git a/panda/VERSION b/panda/VERSION index 0408c30b4..24e56e03c 100644 --- a/panda/VERSION +++ b/panda/VERSION @@ -1 +1 @@ -v1.2.0 \ No newline at end of file +v1.2.1 \ No newline at end of file diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h index b837094c9..9f02c2eba 100644 --- a/panda/board/drivers/can.h +++ b/panda/board/drivers/can.h @@ -3,6 +3,8 @@ #define ALL_CAN_BUT_MAIN_SILENT 0xFE #define ALL_CAN_LIVE 0 +#include "lline_relay.h" + int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_SILENT; // ********************* instantiate queues ********************* @@ -23,6 +25,11 @@ can_buffer(tx2_q, 0x100) can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q}; #endif +#ifdef PANDA +// Forward declare +void power_save_reset_timer(); +#endif + // ********************* interrupt safe queue ********************* int can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { @@ -213,7 +220,7 @@ void can_init(uint8_t can_number) { CAN->FMR &= ~(CAN_FMR_FINIT); // enable certain CAN interrupts - CAN->IER |= CAN_IER_TMEIE | CAN_IER_FMPIE0; + CAN->IER |= CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_WKUIE; switch (can_number) { case 0: @@ -293,7 +300,6 @@ void can_set_gmlan(int bus) { void can_sce(CAN_TypeDef *CAN) { enter_critical_section(); - can_err_cnt += 1; #ifdef DEBUG if (CAN==CAN1) puts("CAN1: "); if (CAN==CAN2) puts("CAN2: "); @@ -315,16 +321,32 @@ void can_sce(CAN_TypeDef *CAN) { uint8_t can_number = CAN_NUM_FROM_CANIF(CAN); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - if (can_autobaud_enabled[bus_number] && (CAN->ESR & CAN_ESR_LEC)) { - can_autobaud_speed_increment(can_number); - can_set_speed(can_number); + + if (CAN->MSR & CAN_MSR_WKUI) { + //Waking from sleep + #ifdef DEBUG + puts("WAKE\n"); + #endif + set_can_enable(CAN, 1); + CAN->MSR &= ~(CAN_MSR_WKUI); + CAN->MSR = CAN->MSR; +#ifdef PANDA + power_save_reset_timer(); +#endif + } else { + can_err_cnt += 1; + + + if (can_autobaud_enabled[bus_number] && (CAN->ESR & CAN_ESR_LEC)) { + can_autobaud_speed_increment(can_number); + can_set_speed(can_number); + } + + // clear current send + CAN->TSR |= CAN_TSR_ABRQ0; + CAN->MSR &= ~(CAN_MSR_ERRI); + CAN->MSR = CAN->MSR; } - - // clear current send - CAN->TSR |= CAN_TSR_ABRQ0; - CAN->MSR &= ~(CAN_MSR_ERRI); - CAN->MSR = CAN->MSR; - exit_critical_section(); } @@ -332,6 +354,9 @@ void can_sce(CAN_TypeDef *CAN) { void process_can(uint8_t can_number) { if (can_number == 0xff) return; +#ifdef PANDA + power_save_reset_timer(); +#endif enter_critical_section(); @@ -375,6 +400,13 @@ void process_can(uint8_t can_number) { } if (can_pop(can_queues[bus_number], &to_send)) { + if (CAN->MCR & CAN_MCR_SLEEP) { + set_can_enable(CAN, 1); + CAN->MCR &= ~(CAN_MCR_SLEEP); + CAN->MCR |= CAN_MCR_INRQ; + while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); + CAN->MCR &= ~(CAN_MCR_INRQ); + } can_tx_cnt += 1; // only send if we have received a packet CAN->sTxMailBox[0].TDLR = to_send.RDLR; @@ -390,6 +422,9 @@ void process_can(uint8_t can_number) { // CAN receive handlers // blink blue when we are receiving CAN messages void can_rx(uint8_t can_number) { + #ifdef PANDA + power_save_reset_timer(); + #endif CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); while (CAN->RF0R & CAN_RF0R_FMP0) { @@ -420,14 +455,16 @@ void can_rx(uint8_t can_number) { // forwarding (panda only) #ifdef PANDA - int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push); - if (bus_fwd_num != -1) { - CAN_FIFOMailBox_TypeDef to_send; - to_send.RIR = to_push.RIR | 1; // TXRQ - to_send.RDTR = to_push.RDTR; - to_send.RDLR = to_push.RDLR; - to_send.RDHR = to_push.RDHR; - can_send(&to_send, bus_fwd_num); + if ((get_lline_status() != 0) || !relay_control) { //Relay engaged or relay isn't controlled, allow fwd + int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push); + if (bus_fwd_num != -1) { + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = to_push.RIR | 1; // TXRQ + to_send.RDTR = to_push.RDTR; + to_send.RDLR = to_push.RDLR; + to_send.RDHR = to_push.RDHR; + can_send(&to_send, bus_fwd_num); + } } #endif diff --git a/panda/board/drivers/lline_relay.h b/panda/board/drivers/lline_relay.h new file mode 100644 index 000000000..217e7dfe3 --- /dev/null +++ b/panda/board/drivers/lline_relay.h @@ -0,0 +1,88 @@ +#ifdef PANDA + +int relay_control = 0; // True if relay is controlled through l-line + +/* Conrol a relay connected to l-line pin */ + +// 160us cycles, 1 high, 25 low + +volatile int turn_on_relay = 0; +volatile int on_cycles = 25; + +//5s timeout +#define LLINE_TIMEOUT_CYCLES 31250 +volatile int timeout_cycles = LLINE_TIMEOUT_CYCLES; + +void TIM5_IRQHandler(void) { + if (TIM5->SR & TIM_SR_UIF) { + on_cycles--; + timeout_cycles--; + if (timeout_cycles == 0) { + turn_on_relay = 0; + } + if (on_cycles > 0) { + if (turn_on_relay) { + set_gpio_output(GPIOC, 10, 0); + } + } + else { + set_gpio_output(GPIOC, 10, 1); + on_cycles = 25; + } + } + TIM5->ARR = 160-1; + TIM5->SR = 0; +} + +void lline_relay_init (void) { + set_lline_output(0); + relay_control = 1; + set_gpio_output(GPIOC, 10, 1); + + // setup + TIM5->PSC = 48-1; // tick on 1 us + TIM5->CR1 = TIM_CR1_CEN; // enable + TIM5->ARR = 50-1; // 50 us + TIM5->DIER = TIM_DIER_UIE; // update interrupt + TIM5->CNT = 0; + + NVIC_EnableIRQ(TIM5_IRQn); + +#ifdef DEBUG + puts("INIT LLINE\n"); + puts(" SR "); + putui(TIM5->SR); + puts(" PSC "); + putui(TIM5->PSC); + puts(" CR1 "); + putui(TIM5->CR1); + puts(" ARR "); + putui(TIM5->ARR); + puts(" DIER "); + putui(TIM5->DIER); + puts(" SR "); + putui(TIM5->SR); + puts(" CNT "); + putui(TIM5->CNT); + puts("\n"); +#endif +} + +void lline_relay_release (void) { + set_lline_output(0); + relay_control = 0; + puts("RELEASE LLINE\n"); + set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3); + NVIC_DisableIRQ(TIM5_IRQn); +} + +void set_lline_output(int to_set) { + timeout_cycles = LLINE_TIMEOUT_CYCLES; + turn_on_relay = to_set; +} + +int get_lline_status() { + return turn_on_relay; +} + +#endif diff --git a/panda/board/gpio.h b/panda/board/gpio.h index 7bd24ecbb..ede6c9b3e 100644 --- a/panda/board/gpio.h +++ b/panda/board/gpio.h @@ -120,6 +120,8 @@ void periph_init() { RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; + RCC->APB1ENR |= RCC_APB1ENR_TIM6EN; RCC->APB2ENR |= RCC_APB2ENR_USART1EN; RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN; RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; @@ -390,7 +392,9 @@ void gpio_init() { set_gpio_output(GPIOA, 14, 1); // C10,C11: L-Line setup on USART 3 - set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3); + // LLine now used for relay output + set_gpio_output(GPIOC, 10, 1); + //set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3); set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3); set_gpio_pullup(GPIOC, 11, PULL_UP); #endif @@ -475,4 +479,3 @@ void early() { enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; } } - diff --git a/panda/board/main.c b/panda/board/main.c index 8ee1bb0c8..478f8b942 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -20,6 +20,8 @@ #include "drivers/spi.h" #include "drivers/timer.h" +#include "power_saving.h" + // ***************************** fan ***************************** @@ -141,6 +143,7 @@ void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) { uart_ring *ur = get_ring_by_number(usbdata[0]); if (!ur) return; if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) { + if (ur == &esp_ring) power_save_reset_timer(); for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i])); } } @@ -160,6 +163,14 @@ void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) { uint8_t bus_number = (to_push.RDTR >> 4) & CAN_BUS_NUM_MASK; can_send(&to_push, bus_number); + + #ifdef PANDA + // Enable relay on can message if allowed. + // Temporary until OP has support for relay + if (safety_relay_hook()) { + set_lline_output(1); + } + #endif } } @@ -441,6 +452,16 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { } break; } + // **** 0xf3: set l-line relay + case 0xf3: + { + #ifdef PANDA + if (safety_relay_hook()) { + set_lline_output(setup->b.wValue.w == 1); + } + #endif + break; + } default: puts("NO HANDLER "); puth(setup->b.bRequest); @@ -572,6 +593,9 @@ int main() { #ifdef PANDA spi_init(); #endif +#ifdef DEBUG + puts("DEBUG ENABLED\n"); +#endif // set PWM fan_init(); @@ -581,6 +605,8 @@ int main() { __enable_irq(); + power_save_init(); + // if the error interrupt is enabled to quickly when the CAN bus is active // something bad happens and you can't connect to the device over USB delay(10000000); diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c index 13a221a87..6b55ca780 100644 --- a/panda/board/pedal/main.c +++ b/panda/board/pedal/main.c @@ -295,6 +295,7 @@ int main() { puts("**** INTERRUPTS ON ****\n"); __enable_irq(); + // main pedal loop while (1) { pedal(); @@ -302,4 +303,3 @@ int main() { return 0; } - diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h new file mode 100644 index 000000000..c6f83457f --- /dev/null +++ b/panda/board/power_saving.h @@ -0,0 +1,157 @@ +#define POWER_SAVE_STATUS_DISABLED 0 +//Moving to enabled, but can wakeup not yet enabled +#define POWER_SAVE_STATUS_SWITCHING 1 +#define POWER_SAVE_STATUS_ENABLED 2 + +volatile int power_save_status = POWER_SAVE_STATUS_DISABLED; + +void power_save_enable(void) { + power_save_status = POWER_SAVE_STATUS_SWITCHING; + puts("Saving power\n"); + //Turn off can transciever + set_can_enable(CAN1, 0); + set_can_enable(CAN2, 0); +#ifdef PANDA + set_can_enable(CAN3, 0); +#endif + + //Turn off GMLAN + set_gpio_output(GPIOB, 14, 0); + set_gpio_output(GPIOB, 15, 0); + +#ifdef PANDA + //Turn off LIN K + if (revision == PANDA_REV_C) { + set_gpio_output(GPIOB, 7, 0); // REV C + } else { + set_gpio_output(GPIOB, 4, 0); // REV AB + } + // LIN L + set_gpio_output(GPIOA, 14, 0); +#endif + + if (is_grey_panda) { + char* UBLOX_SLEEP_MSG = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78"; + int len = 12; + uart_ring *ur = get_ring_by_number(1); + for (int i = 0; i < len; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i])); + } + + //Setup timer for can enable + TIM6->PSC = 48-1; // tick on 1 us + + TIM6->ARR = 12; // 12us + // Enable, One-Pulse Mode, Only overflow interrupt + TIM6->CR1 = TIM_CR1_CEN | TIM_CR1_OPM | TIM_CR1_URS; + TIM6->EGR = TIM_EGR_UG; + TIM6->CR1 |= TIM_CR1_CEN; +} + +void power_save_enable_can_wake(void) { + // CAN Automatic Wake must be done a little while after the sleep + // On some cars turning off the can transciver can trigger the wakeup + power_save_status = POWER_SAVE_STATUS_ENABLED; + puts("Turning can off\n"); + CAN1->MCR |= CAN_MCR_SLEEP; + CAN1->MCR |= CAN_MCR_AWUM; + + CAN2->MCR |= CAN_MCR_SLEEP; + CAN2->MCR |= CAN_MCR_AWUM; +#ifdef PANDA + CAN3->MCR |= CAN_MCR_SLEEP; + CAN3->MCR |= CAN_MCR_AWUM; +#endif + + //set timer back + TIM6->PSC = 48000-1; // tick on 1 ms + TIM6->ARR = 10000; // 10s + // Enable, One-Pulse Mode, Only overflow interrupt + TIM6->CR1 = TIM_CR1_OPM | TIM_CR1_URS; + TIM6->EGR = TIM_EGR_UG; +} + +void power_save_disable(void) { + power_save_status = POWER_SAVE_STATUS_DISABLED; + puts("not Saving power\n"); + TIM6->CR1 |= TIM_CR1_CEN; //Restart timer + TIM6->CNT = 0; + + //Turn on can + set_can_enable(CAN1, 1); + set_can_enable(CAN2, 1); + +#ifdef PANDA + set_can_enable(CAN3, 1); +#endif + + //Turn on GMLAN + set_gpio_output(GPIOB, 14, 1); + set_gpio_output(GPIOB, 15, 1); + +#ifdef PANDA + //Turn on LIN K + if (revision == PANDA_REV_C) { + set_gpio_output(GPIOB, 7, 1); // REV C + } else { + set_gpio_output(GPIOB, 4, 1); // REV AB + } + // LIN L + set_gpio_output(GPIOA, 14, 1); +#endif + + if (is_grey_panda) { + char* UBLOX_WAKE_MSG = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a"; + int len = 12; + uart_ring *ur = get_ring_by_number(1); + for (int i = 0; i < len; i++) while (!putc(ur, UBLOX_WAKE_MSG[i])); + } + + //set timer back + TIM6->PSC = 48000-1; // tick on 1 ms + TIM6->ARR = 10000; // 10s + // Enable, One-Pulse Mode, Only overflow interrupt + TIM6->CR1 = TIM_CR1_CEN | TIM_CR1_OPM | TIM_CR1_URS; + TIM6->EGR = TIM_EGR_UG; + TIM6->CR1 |= TIM_CR1_CEN; +} + + + +// Reset timer when activity +void power_save_reset_timer() { + TIM6->CNT = 0; + if (power_save_status != POWER_SAVE_STATUS_DISABLED){ + power_save_disable(); + } +} + +void power_save_init(void) { + puts("Saving power init\n"); + TIM6->PSC = 48000-1; // tick on 1 ms + + + TIM6->ARR = 10000; // 10s + // Enable, One-Pulse Mode, Only overflow interrupt + TIM6->CR1 = TIM_CR1_CEN | TIM_CR1_OPM | TIM_CR1_URS; + TIM6->EGR = TIM_EGR_UG; + NVIC_EnableIRQ(TIM6_DAC_IRQn); + puts("Saving power init done\n"); + TIM6->DIER = TIM_DIER_UIE; + TIM6->CR1 |= TIM_CR1_CEN; +} + +void TIM6_DAC_IRQHandler(void) { + //Timeout switch to power saving mode. + if (TIM6->SR & TIM_SR_UIF) { + TIM6->SR = 0; +#ifdef EON + if (power_save_status == POWER_SAVE_STATUS_DISABLED) { + power_save_enable(); + } else if (power_save_status == POWER_SAVE_STATUS_SWITCHING) { + power_save_enable_can_wake(); + } +#endif + } else { + TIM6->CR1 |= TIM_CR1_CEN; + } +} diff --git a/panda/board/safety.h b/panda/board/safety.h index 6e5dc8e36..5543333e3 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -29,6 +29,10 @@ int driver_limit_check(int val, int val_last, struct sample_t *val_driver, int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA); #ifdef PANDA float interpolate(struct lookup_t xy, float x); + +void lline_relay_init (void); +void lline_relay_release (void); +void set_lline_output(int to_set); #endif typedef void (*safety_hook_init)(int16_t param); @@ -37,6 +41,7 @@ typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send); typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len); typedef int (*ign_hook)(); typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); +typedef int (*relay_hook)(); typedef struct { safety_hook_init init; @@ -45,6 +50,7 @@ typedef struct { tx_hook tx; tx_lin_hook tx_lin; fwd_hook fwd; + relay_hook relay; } safety_hooks; // This can be set by the safety hooks. @@ -91,6 +97,10 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return current_hooks->fwd(bus_num, to_fwd); } +int safety_relay_hook(void) { + return current_hooks->relay(); +} + typedef struct { uint16_t id; const safety_hooks *hooks; diff --git a/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h index 2a2d8b985..0e6636a1a 100644 --- a/panda/board/safety/safety_cadillac.h +++ b/panda/board/safety/safety_cadillac.h @@ -115,6 +115,9 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { static void cadillac_init(int16_t param) { controls_allowed = 0; cadillac_ign = 0; + #ifdef PANDA + lline_relay_release(); + #endif } static int cadillac_ign_hook() { @@ -128,4 +131,5 @@ const safety_hooks cadillac_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = cadillac_ign_hook, .fwd = alloutput_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index b1c6a743f..a8826b915 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -5,7 +5,7 @@ const int CHRYSLER_MAX_RATE_UP = 3; const int CHRYSLER_MAX_RATE_DOWN = 3; const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor -int chrysler_camera_detected = 0; +int chrysler_camera_detected = 0; // is giraffe switch 2 high? int chrysler_rt_torque_last = 0; int chrysler_desired_torque_last = 0; int chrysler_cruise_engaged_last = 0; @@ -125,12 +125,33 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return true; } +static void chrysler_init(int16_t param) { + chrysler_camera_detected = 0; + #ifdef PANDA + lline_relay_release(); + #endif +} + +static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + int32_t addr = to_fwd->RIR >> 21; + // forward CAN 0 -> 2 so stock LKAS camera sees messages + if (bus_num == 0 && !chrysler_camera_detected) { + return 2; + } + // forward all messages from camera except LKAS_COMMAND and LKAS_HUD + if (bus_num == 2 && !chrysler_camera_detected && addr != 658 && addr != 678) { + return 0; + } + return -1; // do not forward +} + const safety_hooks chrysler_hooks = { - .init = nooutput_init, + .init = chrysler_init, .rx = chrysler_rx_hook, .tx = chrysler_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, - .fwd = nooutput_fwd_hook, + .fwd = chrysler_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h index 196df65d2..88d31a7de 100644 --- a/panda/board/safety/safety_defaults.h +++ b/panda/board/safety/safety_defaults.h @@ -8,6 +8,9 @@ int default_ign_hook() { static void nooutput_init(int16_t param) { controls_allowed = 0; + #ifdef PANDA + lline_relay_release(); + #endif } static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -22,6 +25,10 @@ static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } +static int nooutput_relay_hook(int to_set) { + return false; +} + const safety_hooks nooutput_hooks = { .init = nooutput_init, .rx = default_rx_hook, @@ -29,12 +36,16 @@ const safety_hooks nooutput_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = nooutput_fwd_hook, + .relay = nooutput_relay_hook, }; // *** all output safety mode *** static void alloutput_init(int16_t param) { controls_allowed = 1; + #ifdef PANDA + lline_relay_release(); + #endif } static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -49,6 +60,10 @@ static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } +static int alloutput_relay_hook(int to_set) { + return true; +} + const safety_hooks alloutput_hooks = { .init = alloutput_init, .rx = default_rx_hook, @@ -56,4 +71,5 @@ const safety_hooks alloutput_hooks = { .tx_lin = alloutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = alloutput_fwd_hook, + .relay = alloutput_relay_hook, }; diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h index 98dce6532..e89783bcc 100644 --- a/panda/board/safety/safety_elm327.h +++ b/panda/board/safety/safety_elm327.h @@ -42,4 +42,5 @@ const safety_hooks elm327_hooks = { .tx_lin = elm327_tx_lin_hook, .ignition = default_ign_hook, .fwd = elm327_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 075029fb6..1d3f16d06 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -90,4 +90,5 @@ const safety_hooks ford_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = nooutput_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index f35b26b4e..396fb8770 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -228,6 +228,9 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { static void gm_init(int16_t param) { controls_allowed = 0; gm_ignition_started = 0; + #ifdef PANDA + lline_relay_release(); + #endif } static int gm_ign_hook() { @@ -241,5 +244,5 @@ const safety_hooks gm_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = gm_ign_hook, .fwd = nooutput_fwd_hook, + .relay = nooutput_relay_hook, }; - diff --git a/panda/board/safety/safety_gm_ascm.h b/panda/board/safety/safety_gm_ascm.h index 70a042ec5..b145466d9 100644 --- a/panda/board/safety/safety_gm_ascm.h +++ b/panda/board/safety/safety_gm_ascm.h @@ -48,5 +48,6 @@ const safety_hooks gm_ascm_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = gm_ascm_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index fbee6cfe8..40c6917b0 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -136,6 +136,9 @@ static void honda_init(int16_t param) { controls_allowed = 0; bosch_hardware = false; honda_alt_brake_msg = false; + #ifdef PANDA + lline_relay_release(); + #endif } static void honda_bosch_init(int16_t param) { @@ -143,6 +146,9 @@ static void honda_bosch_init(int16_t param) { bosch_hardware = true; // Checking for alternate brake override from safety parameter honda_alt_brake_msg = param == 1 ? true : false; + #ifdef PANDA + lline_relay_release(); + #endif } static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { @@ -176,6 +182,7 @@ const safety_hooks honda_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = honda_fwd_hook, + .relay = nooutput_relay_hook, }; const safety_hooks honda_bosch_hooks = { @@ -185,4 +192,5 @@ const safety_hooks honda_bosch_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = honda_bosch_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index b67632141..9470e34d1 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -152,6 +152,9 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { static void hyundai_init(int16_t param) { controls_allowed = 0; hyundai_giraffe_switch_2 = 0; + #ifdef PANDA + lline_relay_release(); + #endif } const safety_hooks hyundai_hooks = { @@ -161,4 +164,5 @@ const safety_hooks hyundai_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = hyundai_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 00fe1abf9..9fedf16c6 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -14,6 +14,11 @@ int subaru_desired_torque_last = 0; uint32_t subaru_ts_last = 0; struct sample_t subaru_torque_driver; // last few driver torques measured +static void subaru_init(int16_t param) { + #ifdef PANDA + lline_relay_init(); + #endif +} static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus_number = (to_push->RDTR >> 4) & 0xFF; @@ -100,6 +105,7 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { // forward CAN 0 > 1 if (bus_num == 0) { + return 2; // ES CAN } // forward CAN 1 > 0, except ES_LKAS @@ -113,6 +119,10 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (addr == 0x122) { return -1; } + // ES Distance + if (addr == 545) { + return -1; + } return 0; // Main CAN } @@ -122,10 +132,11 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { } const safety_hooks subaru_hooks = { - .init = nooutput_init, + .init = subaru_init, .rx = subaru_rx_hook, .tx = subaru_tx_hook, .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = subaru_fwd_hook, + .relay = alloutput_relay_hook, }; diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 78c450e85..882a509f5 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -230,6 +230,9 @@ static void tesla_init(int16_t param) controls_allowed = 0; tesla_ignition_started = 0; gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled + #ifdef PANDA + lline_relay_release(); + #endif } static int tesla_ign_hook() @@ -284,4 +287,5 @@ const safety_hooks tesla_hooks = { .tx_lin = tesla_tx_lin_hook, .ignition = tesla_ign_hook, .fwd = tesla_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 50d457ab6..02fcb36e5 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -160,6 +160,9 @@ static void toyota_init(int16_t param) { toyota_giraffe_switch_1 = 0; toyota_camera_forwarded = 0; toyota_dbc_eps_torque_factor = param; + #ifdef PANDA + lline_relay_release(); + #endif } static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { @@ -181,6 +184,7 @@ const safety_hooks toyota_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = toyota_fwd_hook, + .relay = nooutput_relay_hook, }; static void toyota_nolimits_init(int16_t param) { @@ -189,6 +193,9 @@ static void toyota_nolimits_init(int16_t param) { toyota_giraffe_switch_1 = 0; toyota_camera_forwarded = 0; toyota_dbc_eps_torque_factor = param; + #ifdef PANDA + lline_relay_release(); + #endif } const safety_hooks toyota_nolimits_hooks = { @@ -198,4 +205,5 @@ const safety_hooks toyota_nolimits_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = toyota_fwd_hook, + .relay = nooutput_relay_hook, }; diff --git a/panda/board/safety/safety_toyota_ipas.h b/panda/board/safety/safety_toyota_ipas.h index ff4158e3c..13e664c0f 100644 --- a/panda/board/safety/safety_toyota_ipas.h +++ b/panda/board/safety/safety_toyota_ipas.h @@ -152,5 +152,5 @@ const safety_hooks toyota_ipas_hooks = { .tx_lin = nooutput_tx_lin_hook, .ignition = default_ign_hook, .fwd = toyota_fwd_hook, + .relay = nooutput_relay_hook, }; - diff --git a/panda/boardesp/get_sdk_ci.sh b/panda/boardesp/get_sdk_ci.sh new file mode 100755 index 000000000..b11cb099a --- /dev/null +++ b/panda/boardesp/get_sdk_ci.sh @@ -0,0 +1,5 @@ +#!/bin/bash +git clone --recursive https://github.com/pfalcon/esp-open-sdk.git +cd esp-open-sdk +git checkout 03f5e898a059451ec5f3de30e7feff30455f7cec +LD_LIBRARY_PATH="" make STANDALONE=y diff --git a/panda/python/__init__.py b/panda/python/__init__.py index b3610e6c3..8c0bc5a9a 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -182,6 +182,7 @@ class Panda(object): traceback.print_exc() if wait == False or self._handle != None: break + context = usb1.USBContext() #New context needed so new devices show up assert(self._handle != None) print("connected") @@ -280,11 +281,14 @@ class Panda(object): if reconnect: self.reconnect() - def recover(self): + def recover(self, timeout=None): self.reset(enter_bootloader=True) + t_start = time.time() while len(PandaDFU.list()) == 0: print("waiting for DFU...") time.sleep(0.1) + if timeout is not None and (time.time() - t_start) > timeout: + return False dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(self._serial)) dfu.recover() @@ -292,6 +296,7 @@ class Panda(object): # reflash after recover self.connect(True, True) self.flash() + return True @staticmethod def flash_ota_st(): @@ -300,8 +305,9 @@ class Panda(object): return ret==0 @staticmethod - def flash_ota_wifi(): - ret = os.system("cd %s && make clean && make ota" % (os.path.join(BASEDIR, "boardesp"))) + def flash_ota_wifi(release=False): + release_str = "RELEASE=1" if release else "" + ret = os.system("cd {} && make clean && {} make ota".format(os.path.join(BASEDIR, "boardesp"),release_str)) time.sleep(1) return ret==0 @@ -386,10 +392,17 @@ class Panda(object): elif bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]: self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 1, bus, b'') + def set_lline_relay(self, enable): + self._handle.controlWrite(Panda.REQUEST_OUT, 0xf3, int(enable), 0, b'') + def set_can_loopback(self, enable): # set can loopback mode for all buses self._handle.controlWrite(Panda.REQUEST_OUT, 0xe5, int(enable), 0, b'') + def set_can_enable(self, bus_num, enable): + # sets the can transciever enable pin + self._handle.controlWrite(Panda.REQUEST_OUT, 0xf4, int(bus_num), int(enable), b'') + def set_can_speed_kbps(self, bus, speed): self._handle.controlWrite(Panda.REQUEST_OUT, 0xde, bus, int(speed*10), b'') diff --git a/panda/requirements.txt b/panda/requirements.txt index e968bfb6b..ad6b4c76e 100644 --- a/panda/requirements.txt +++ b/panda/requirements.txt @@ -1,4 +1,7 @@ -libusb1 +libusb1 == 1.6.6 hexdump pycrypto tqdm +nose +parameterized +requests diff --git a/panda/run_automated_tests.sh b/panda/run_automated_tests.sh index d7b754190..63d9b6531 100755 --- a/panda/run_automated_tests.sh +++ b/panda/run_automated_tests.sh @@ -1,3 +1,9 @@ #!/bin/bash -PYTHONPATH="." nosetests -x -s tests/automated/$1*.py +TEST_FILENAME=${TEST_FILENAME:-nosetests.xml} +if [ ! -f "/EON" ]; then + TESTSUITE_NAME="Panda_Test-EON" +else + TESTSUITE_NAME="Panda_Test-DEV" +fi +PYTHONPATH="." nosetests -v --with-xunit --xunit-file=./$TEST_FILENAME --xunit-testsuite-name=$TESTSUITE_NAME -s tests/automated/$1*.py diff --git a/panda/setup.py b/panda/setup.py index 312fdca42..2acd9b9e1 100644 --- a/panda/setup.py +++ b/panda/setup.py @@ -46,7 +46,7 @@ setup( platforms='any', license='MIT', install_requires=[ - 'libusb1 >= 1.6.4', + 'libusb1 == 1.6.6', 'hexdump >= 3.3', 'pycrypto >= 2.6.1', 'tqdm >= 4.14.0', diff --git a/panda/tests/automated/1_program.py b/panda/tests/automated/1_program.py index 7da801bcd..1e0beb8ae 100644 --- a/panda/tests/automated/1_program.py +++ b/panda/tests/automated/1_program.py @@ -1,11 +1,15 @@ import os from panda import Panda +from helpers import panda_color_to_serial, test_white_and_grey -def test_recover(): - p = Panda() - p.recover() +@test_white_and_grey +@panda_color_to_serial +def test_recover(serial=None): + p = Panda(serial=serial) + assert p.recover(timeout=30) -def test_flash(): - p = Panda() +@test_white_and_grey +@panda_color_to_serial +def test_flash(serial=None): + p = Panda(serial=serial) p.flash() - diff --git a/panda/tests/automated/2_usb_to_can.py b/panda/tests/automated/2_usb_to_can.py index 481564240..7860d3290 100644 --- a/panda/tests/automated/2_usb_to_can.py +++ b/panda/tests/automated/2_usb_to_can.py @@ -3,14 +3,16 @@ import os import sys import time from panda import Panda -from nose.tools import timed, assert_equal, assert_less, assert_greater -from helpers import time_many_sends, connect_wo_esp +from nose.tools import assert_equal, assert_less, assert_greater +from helpers import time_many_sends, connect_wo_esp, test_white_and_grey, panda_color_to_serial SPEED_NORMAL = 500 SPEED_GMLAN = 33.3 -def test_can_loopback(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_can_loopback(serial=None): + p = connect_wo_esp(serial) # enable output mode p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) @@ -42,8 +44,10 @@ def test_can_loopback(): assert 0x1aa == sr[0][0] == lb[0][0] assert "message" == sr[0][2] == lb[0][2] -def test_safety_nooutput(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_safety_nooutput(serial=None): + p = connect_wo_esp(serial) # enable output mode p.set_safety_mode(Panda.SAFETY_NOOUTPUT) @@ -59,8 +63,10 @@ def test_safety_nooutput(): r = p.can_recv() assert len(r) == 0 -def test_reliability(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_reliability(serial=None): + p = connect_wo_esp(serial) LOOP_COUNT = 100 MSG_COUNT = 100 @@ -97,8 +103,10 @@ def test_reliability(): sys.stdout.write("P") sys.stdout.flush() -def test_throughput(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_throughput(serial=None): + p = connect_wo_esp(serial) # enable output mode p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) @@ -120,8 +128,10 @@ def test_throughput(): print("loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct)) -def test_gmlan(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_gmlan(serial=None): + p = connect_wo_esp(serial) if p.legacy: return @@ -135,7 +145,7 @@ def test_gmlan(): p.set_can_speed_kbps(1, SPEED_NORMAL) p.set_can_speed_kbps(2, SPEED_NORMAL) p.set_can_speed_kbps(3, SPEED_GMLAN) - + # set gmlan on CAN2 for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]: p.set_gmlan(bus) @@ -150,8 +160,10 @@ def test_gmlan(): print("%d: %.2f kbps vs %.2f kbps" % (bus, comp_kbps_gmlan, comp_kbps_normal)) -def test_gmlan_bad_toggle(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_gmlan_bad_toggle(serial=None): + p = connect_wo_esp(serial) if p.legacy: return @@ -178,9 +190,10 @@ def test_gmlan_bad_toggle(): # this will fail if you have hardware serial connected -def test_serial_debug(): - p = connect_wo_esp() +@test_white_and_grey +@panda_color_to_serial +def test_serial_debug(serial=None): + p = connect_wo_esp(serial) junk = p.serial_read(Panda.SERIAL_DEBUG) p.call_control_api(0xc0) assert(p.serial_read(Panda.SERIAL_DEBUG).startswith("can ")) - diff --git a/panda/tests/automated/3_wifi.py b/panda/tests/automated/3_wifi.py index c6f4154ba..6578903aa 100644 --- a/panda/tests/automated/3_wifi.py +++ b/panda/tests/automated/3_wifi.py @@ -1,33 +1,60 @@ from __future__ import print_function import os +import time from panda import Panda -from helpers import connect_wifi +from helpers import connect_wifi, test_white, test_white_and_grey, panda_color_to_serial import requests -def test_get_serial(): - p = Panda() +@test_white_and_grey +@panda_color_to_serial +def test_get_serial(serial=None): + p = Panda(serial) print(p.get_serial()) -def test_get_serial_in_flash_mode(): - p = Panda() +@test_white_and_grey +@panda_color_to_serial +def test_get_serial_in_flash_mode(serial=None): + p = Panda(serial) p.reset(enter_bootstub=True) assert(p.bootstub) print(p.get_serial()) p.reset() -def test_connect_wifi(): - connect_wifi() +@test_white +@panda_color_to_serial +def test_connect_wifi(serial=None): + connect_wifi(serial) -def test_flash_wifi(): - Panda.flash_ota_wifi() - connect_wifi() +@test_white +@panda_color_to_serial +def test_flash_wifi(serial=None): + connect_wifi(serial) + assert Panda.flash_ota_wifi(release=True), "OTA Wifi Flash Failed" + connect_wifi(serial) -def test_wifi_flash_st(): - Panda.flash_ota_st() +@test_white +@panda_color_to_serial +def test_wifi_flash_st(serial=None): + connect_wifi(serial) + assert Panda.flash_ota_st(), "OTA ST Flash Failed" + connected = False + st = time.time() + while not connected and (time.time() - st) < 20: + try: + p = Panda(serial=serial) + p.get_serial() + connected = True + except: + time.sleep(1) -def test_webpage_fetch(): + if not connected: + assert False, "Panda failed to connect on USB after flashing" + +@test_white +@panda_color_to_serial +def test_webpage_fetch(serial=None): + connect_wifi(serial) r = requests.get("http://192.168.0.10/") print(r.text) assert "This is your comma.ai panda" in r.text - diff --git a/panda/tests/automated/4_wifi_functionality.py b/panda/tests/automated/4_wifi_functionality.py index 68686008e..0cf42d1f3 100644 --- a/panda/tests/automated/4_wifi_functionality.py +++ b/panda/tests/automated/4_wifi_functionality.py @@ -1,17 +1,22 @@ from __future__ import print_function import time from panda import Panda -from helpers import time_many_sends, connect_wifi +from helpers import time_many_sends, connect_wifi, test_white, panda_color_to_serial from nose.tools import timed, assert_equal, assert_less, assert_greater -def test_get_serial_wifi(): - connect_wifi() +@test_white +@panda_color_to_serial +def test_get_serial_wifi(serial=None): + connect_wifi(serial) p = Panda("WIFI") print(p.get_serial()) -def test_throughput(): - p = Panda() +@test_white +@panda_color_to_serial +def test_throughput(serial=None): + connect_wifi(serial) + p = Panda(serial) # enable output mode p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) @@ -24,7 +29,7 @@ def test_throughput(): for speed in [100,250,500,750,1000]: # set bus 0 speed to speed p.set_can_speed_kbps(0, speed) - time.sleep(0.05) + time.sleep(0.1) comp_kbps = time_many_sends(p, 0) @@ -35,8 +40,11 @@ def test_throughput(): print("WIFI loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct)) -def test_recv_only(): - p = Panda() +@test_white +@panda_color_to_serial +def test_recv_only(serial=None): + connect_wifi(serial) + p = Panda(serial) p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) p.set_can_loopback(True) pwifi = Panda("WIFI") @@ -49,4 +57,3 @@ def test_recv_only(): saturation_pct = (comp_kbps/speed) * 100.0 print("HT WIFI loopback %d messages at speed %d, comp speed is %.2f, percent %.2f" % (msg_count, speed, comp_kbps, saturation_pct)) - diff --git a/panda/tests/automated/5_wifi_udp.py b/panda/tests/automated/5_wifi_udp.py index 7d6ccba66..d55baa659 100644 --- a/panda/tests/automated/5_wifi_udp.py +++ b/panda/tests/automated/5_wifi_udp.py @@ -1,14 +1,16 @@ from __future__ import print_function import sys import time -from helpers import time_many_sends, connect_wifi +from helpers import time_many_sends, connect_wifi, test_white, panda_color_to_serial from panda import Panda, PandaWifiStreaming from nose.tools import timed, assert_equal, assert_less, assert_greater -def test_udp_doesnt_drop(): - connect_wifi() +@test_white +@panda_color_to_serial +def test_udp_doesnt_drop(serial=None): + connect_wifi(serial) - p = Panda() + p = Panda(serial) p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) p.set_can_loopback(True) @@ -18,6 +20,7 @@ def test_udp_doesnt_drop(): break for msg_count in [1, 100]: + saturation_pcts = [] for i in range({1: 0x80, 100: 0x20}[msg_count]): pwifi.kick() @@ -31,9 +34,33 @@ def test_udp_doesnt_drop(): sys.stdout.flush() else: print("UDP WIFI loopback %d messages at speed %d, comp speed is %.2f, percent %.2f" % (msg_count, speed, comp_kbps, saturation_pct)) - assert_greater(saturation_pct, 40) + assert_greater(saturation_pct, 20) #sometimes the wifi can be slow... assert_less(saturation_pct, 100) - print("") - - + saturation_pcts.append(saturation_pct) + if len(saturation_pcts) > 0: + assert_greater(sum(saturation_pcts)/len(saturation_pcts), 60) + time.sleep(5) + usb_ok_cnt = 0 + REQ_USB_OK_CNT = 500 + st = time.time() + msg_id = 0x1bb + bus = 0 + last_missing_msg = 0 + while usb_ok_cnt < REQ_USB_OK_CNT and (time.time() - st) < 40: + p.can_send(msg_id, "message", bus) + time.sleep(0.01) + r = [1] + missing = True + while len(r) > 0: + r = p.can_recv() + r = filter(lambda x: x[3] == bus and x[0] == msg_id, r) + if len(r) > 0: + missing = False + usb_ok_cnt += len(r) + if missing: + last_missing_msg = time.time() + et = time.time() - st + last_missing_msg = last_missing_msg - st + print("waited {} for panda to recv can on usb, {} msgs, last missing at {}".format(et, usb_ok_cnt, last_missing_msg)) + assert usb_ok_cnt >= REQ_USB_OK_CNT, "Unable to recv can on USB after UDP" diff --git a/panda/tests/automated/6_two_panda.py b/panda/tests/automated/6_two_panda.py new file mode 100644 index 000000000..3c29a0e7a --- /dev/null +++ b/panda/tests/automated/6_two_panda.py @@ -0,0 +1,121 @@ +from __future__ import print_function +import time +from panda import Panda +from nose.tools import assert_equal, assert_less, assert_greater +from helpers import time_many_sends, test_two_panda, panda_color_to_serial + +@test_two_panda +@panda_color_to_serial +def test_send_recv(serial_sender=None, serial_reciever=None): + p_send = Panda(serial_sender) + p_recv = Panda(serial_reciever) + + p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p_send.set_can_loopback(False) + + p_recv.set_can_loopback(False) + + assert not p_send.legacy + assert not p_recv.legacy + + p_send.can_send_many([(0x1ba, 0, "message", 0)]*2) + time.sleep(0.05) + p_recv.can_recv() + p_send.can_recv() + + busses = [0,1,2] + + for bus in busses: + for speed in [100, 250, 500, 750, 1000]: + p_send.set_can_speed_kbps(bus, speed) + p_recv.set_can_speed_kbps(bus, speed) + time.sleep(0.05) + + comp_kbps = time_many_sends(p_send, bus, p_recv, two_pandas=True) + + saturation_pct = (comp_kbps/speed) * 100.0 + assert_greater(saturation_pct, 80) + assert_less(saturation_pct, 100) + + print("two pandas bus {}, 100 messages at speed {:4d}, comp speed is {:7.2f}, percent {:6.2f}".format(bus, speed, comp_kbps, saturation_pct)) + +@test_two_panda +@panda_color_to_serial +def test_latency(serial_sender=None, serial_reciever=None): + p_send = Panda(serial_sender) + p_recv = Panda(serial_reciever) + + p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p_send.set_can_loopback(False) + + p_recv.set_can_loopback(False) + + assert not p_send.legacy + assert not p_recv.legacy + + p_send.set_can_speed_kbps(0, 100) + p_recv.set_can_speed_kbps(0, 100) + time.sleep(0.05) + + p_send.can_send_many([(0x1ba, 0, "testmsg", 0)]*10) + time.sleep(0.05) + p_recv.can_recv() + p_send.can_recv() + + busses = [0,1,2] + + for bus in busses: + for speed in [100, 250, 500, 750, 1000]: + p_send.set_can_speed_kbps(bus, speed) + p_recv.set_can_speed_kbps(bus, speed) + time.sleep(0.1) + #clear can buffers + r = [1] + while len(r) > 0: + r = p_send.can_recv() + r = [1] + while len(r) > 0: + r = p_recv.can_recv() + time.sleep(0.05) + + latencies = [] + comp_kbps_list = [] + saturation_pcts = [] + + num_messages = 100 + + for i in range(num_messages): + st = time.time() + p_send.can_send(0x1ab, "message", bus) + r = [] + while len(r) < 1 and (time.time() - st) < 5: + r = p_recv.can_recv() + et = time.time() + r_echo = [] + while len(r_echo) < 1 and (time.time() - st) < 10: + r_echo = p_send.can_recv() + + if len(r) == 0 or len(r_echo) == 0: + print("r: {}, r_echo: {}".format(r, r_echo)) + + assert_equal(len(r),1) + assert_equal(len(r_echo),1) + + et = (et - st)*1000.0 + comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7) / et + latency = et - ((1+11+1+1+1+4+8*8+15+1+1+1+7) / speed) + + assert_less(latency, 5.0) + + saturation_pct = (comp_kbps/speed) * 100.0 + latencies.append(latency) + comp_kbps_list.append(comp_kbps) + saturation_pcts.append(saturation_pct) + + average_latency = sum(latencies)/num_messages + assert_less(average_latency, 1.0) + average_comp_kbps = sum(comp_kbps_list)/num_messages + average_saturation_pct = sum(saturation_pcts)/num_messages + + print("two pandas bus {}, {} message average at speed {:4d}, latency is {:5.3f}ms, comp speed is {:7.2f}, percent {:6.2f}"\ + .format(bus, num_messages, speed, average_latency, average_comp_kbps, average_saturation_pct)) diff --git a/panda/tests/automated/helpers.py b/panda/tests/automated/helpers.py index 1ddced117..9e92f56bf 100644 --- a/panda/tests/automated/helpers.py +++ b/panda/tests/automated/helpers.py @@ -4,12 +4,34 @@ import time import random import subprocess import requests +from functools import wraps from panda import Panda from nose.tools import timed, assert_equal, assert_less, assert_greater +from parameterized import parameterized, param -def connect_wo_esp(): +test_white_and_grey = parameterized([param(panda_color="White"), + param(panda_color="Grey")]) +test_white = parameterized([param(panda_color="White")]) +test_grey = parameterized([param(panda_color="Grey")]) +test_two_panda = parameterized([param(panda_color=["Grey", "White"]), + param(panda_color=["White", "Grey"])]) + +_serials = {} +def get_panda_serial(is_grey=None): + global _serials + if is_grey not in _serials: + for serial in Panda.list(): + p = Panda(serial=serial) + if is_grey is None or p.is_grey() == is_grey: + _serials[is_grey] = serial + return serial + raise IOError("Panda not found. is_grey: {}".format(is_grey)) + else: + return _serials[is_grey] + +def connect_wo_esp(serial=None): # connect to the panda - p = Panda() + p = Panda(serial=serial) # power down the ESP p.set_esp_power(False) @@ -20,15 +42,28 @@ def connect_wo_esp(): return p -def connect_wifi(): - p = Panda() +def connect_wifi(serial=None): + p = Panda(serial=serial) + p.set_esp_power(True) dongle_id, pw = p.get_serial() assert(dongle_id.isalnum()) _connect_wifi(dongle_id, pw) +FNULL = open(os.devnull, 'w') def _connect_wifi(dongle_id, pw, insecure_okay=False): ssid = str("panda-" + dongle_id) + r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT) + if not r: + #Can already ping, try connecting on wifi + try: + p = Panda("WIFI") + p.get_serial() + print("Already connected") + return + except: + pass + print("WIFI: connecting to %s" % ssid) while 1: @@ -39,8 +74,8 @@ def _connect_wifi(dongle_id, pw, insecure_okay=False): cnt = 0 MAX_TRIES = 10 while cnt < MAX_TRIES: - print "WIFI: scanning %d" % cnt - os.system("sudo iwlist %s scanning > /dev/null" % wlan_interface) + print("WIFI: scanning %d" % cnt) + os.system("iwlist %s scanning > /dev/null" % wlan_interface) os.system("nmcli device wifi rescan") wifi_scan = filter(lambda x: ssid in x, subprocess.check_output(["nmcli","dev", "wifi", "list"]).split("\n")) if len(wifi_scan) != 0: @@ -51,45 +86,107 @@ def _connect_wifi(dongle_id, pw, insecure_okay=False): assert cnt < MAX_TRIES if "-pair" in wifi_scan[0]: os.system("nmcli d wifi connect %s-pair" % (ssid)) + connect_cnt = 0 + MAX_TRIES = 20 + while connect_cnt < MAX_TRIES: + connect_cnt += 1 + r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT) + if r: + print("Waiting for panda to ping...") + time.sleep(0.1) + else: + break if insecure_okay: break # fetch webpage - print "connecting to insecure network to secure" - r = requests.get("http://192.168.0.10/") + print("connecting to insecure network to secure") + try: + r = requests.get("http://192.168.0.10/") + except requests.ConnectionError: + r = requests.get("http://192.168.0.10/") assert r.status_code==200 - print "securing" + print("securing") try: r = requests.get("http://192.168.0.10/secure", timeout=0.01) except requests.exceptions.Timeout: + print("timeout http request to secure") pass else: - os.system("nmcli d wifi connect %s password %s" % (ssid, pw)) - break - + ret = os.system("nmcli d wifi connect %s password %s" % (ssid, pw)) + if os.WEXITSTATUS(ret) == 0: + #check ping too + ping_ok = False + connect_cnt = 0 + MAX_TRIES = 10 + while connect_cnt < MAX_TRIES: + connect_cnt += 1 + r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT) + if r: + print("Waiting for panda to ping...") + time.sleep(0.1) + else: + ping_ok = True + break + if ping_ok: + break + # TODO: confirm that it's connected to the right panda -def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None): +def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None, two_pandas=False): if precv == None: precv = p if msg_id == None: msg_id = random.randint(0x100, 0x200) + if p == precv and two_pandas: + raise ValueError("Cannot have two pandas that are the same panda") st = time.time() p.can_send_many([(msg_id, 0, "\xaa"*8, bus)]*msg_count) r = [] + r_echo = [] + r_len_expected = msg_count if two_pandas else msg_count*2 + r_echo_len_exected = msg_count if two_pandas else 0 - while len(r) < (msg_count*2) and (time.time() - st) < 3: + while len(r) < r_len_expected and (time.time() - st) < 5: r.extend(precv.can_recv()) + et = time.time() + if two_pandas: + while len(r_echo) < r_echo_len_exected and (time.time() - st) < 10: + r_echo.extend(p.can_recv()) sent_echo = filter(lambda x: x[3] == 0x80 | bus and x[0] == msg_id, r) - loopback_resp = filter(lambda x: x[3] == bus and x[0] == msg_id, r) + sent_echo.extend(filter(lambda x: x[3] == 0x80 | bus and x[0] == msg_id, r_echo)) + resp = filter(lambda x: x[3] == bus and x[0] == msg_id, r) + leftovers = filter(lambda x: (x[3] != 0x80 | bus and x[3] != bus) or x[0] != msg_id, r) + assert_equal(len(leftovers), 0) + + assert_equal(len(resp), msg_count) assert_equal(len(sent_echo), msg_count) - assert_equal(len(loopback_resp), msg_count) - et = (time.time()-st)*1000.0 + et = (et-st)*1000.0 comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7)*msg_count / et return comp_kbps + +def panda_color_to_serial(fn): + @wraps(fn) + def wrapper(panda_color=None, **kwargs): + pandas_is_grey = [] + if panda_color is not None: + if not isinstance(panda_color, list): + panda_color = [panda_color] + panda_color = [s.lower() for s in panda_color] + for p in panda_color: + if p is None: + pandas_is_grey.append(None) + elif p in ["grey", "gray"]: + pandas_is_grey.append(True) + elif p in ["white"]: + pandas_is_grey.append(False) + else: + raise ValueError("Invalid Panda Color {}".format(p)) + return fn(*[get_panda_serial(is_grey) for is_grey in pandas_is_grey], **kwargs) + return wrapper diff --git a/panda/tests/debug_console.py b/panda/tests/debug_console.py index f4db77f09..0238ed789 100755 --- a/panda/tests/debug_console.py +++ b/panda/tests/debug_console.py @@ -21,6 +21,9 @@ if __name__ == "__main__": pandas = list(map(lambda x: Panda(x, claim=claim), serials)) + if not len(pandas): + sys.exit("no pandas found") + if os.getenv("BAUD") is not None: for panda in pandas: panda.set_uart_baud(port_number, int(os.getenv("BAUD"))) diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c index 9b4c807d7..04e6576fb 100644 --- a/panda/tests/safety/test.c +++ b/panda/tests/safety/test.c @@ -246,3 +246,12 @@ void reset_gmlan_switch_timeout(void){ void gmlan_switch_init(int timeout_enable){ } + +void lline_relay_init (void) { +} + +void lline_relay_release (void) { +} + +void set_lline_output(int to_set) { +} diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt index 48b2ddb31..880073ee7 100644 --- a/requirements_openpilot.txt +++ b/requirements_openpilot.txt @@ -23,6 +23,7 @@ enum34==1.1.6 evdev==0.6.1 fastcluster==1.1.20 filterpy==1.2.4 +hexdump==3.3 ipaddress==1.0.16 json-rpc==1.12.1 libusb1==1.5.0 @@ -30,7 +31,9 @@ lmdb==0.92 mpmath==1.0.0 nose==1.3.7 numpy==1.11.1 +opencv-python==3.4.0.12 pause==0.1.2 +psutil==3.4.2 py==1.4.31 pyOpenSSL==16.0.0 pyasn1-modules==0.0.8 @@ -39,6 +42,7 @@ pycapnp==0.6.3 pycparser==2.18 pycrypto==2.6.1 pyflakes==1.6.0 +pylint==1.8.3 pyopencl==2016.1 pyparsing==2.1.10 #pypcap==1.1.5 needs extra dependencies and is not used diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index f6f9ef231..501806969 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -6,7 +6,7 @@ import time import threading import traceback import zmq -import Queue +import six.moves.queue from jsonrpc import JSONRPCResponseManager, dispatcher from websocket import create_connection, WebSocketTimeoutException @@ -21,8 +21,8 @@ ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4) dispatcher["echo"] = lambda s: s -payload_queue = Queue.Queue() -response_queue = Queue.Queue() +payload_queue = six.moves.queue.Queue() +response_queue = six.moves.queue.Queue() def handle_long_poll(ws): end_event = threading.Event() @@ -52,7 +52,7 @@ def jsonrpc_handler(end_event): data = payload_queue.get(timeout=1) response = JSONRPCResponseManager.handle(data, dispatcher) response_queue.put_nowait(response) - except Queue.Empty: + except six.moves.queue.Empty: pass except Exception as e: cloudlog.exception("athena jsonrpc handler failed") @@ -87,7 +87,7 @@ def ws_send(ws, end_event): try: response = response_queue.get(timeout=1) ws.send(response.json) - except Queue.Empty: + except six.moves.queue.Empty: pass except Exception: traceback.print_exc() diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index fb045bb3a..2ebd2e9ff 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -127,16 +127,13 @@ def boardd_mock_loop(): while 1: tsc = messaging.drain_sock(logcan, wait_for_one=True) - snds = map(lambda x: can_capnp_to_can_list(x.can), tsc) - snd = [] - for s in snds: - snd += s - snd = filter(lambda x: x[-1] <= 1, snd) - can_send_many(snd) + snds = [can_capnp_to_can_list(x.can) for x in tsc] + snds = [x for x in snds if x[-1] <= 1] + can_send_many(snds) # recv @ 100hz can_msgs = can_recv() - print("sent %d got %d" % (len(snd), len(can_msgs))) + print("sent %d got %d" % (len(snds), len(can_msgs))) m = can_list_to_can_capnp(can_msgs) sendcan.send(m.to_bytes()) @@ -220,7 +217,7 @@ def boardd_proxy_loop(rate=200, address="192.168.2.251"): # recv @ 100hz can_msgs = can_recv() #for m in can_msgs: - # print "R:",hex(m[0]), str(m[2]).encode("hex") + # print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) # publish to logger # TODO: refactor for speed @@ -233,7 +230,7 @@ def boardd_proxy_loop(rate=200, address="192.168.2.251"): if tsc is not None: cl = can_capnp_to_can_list(tsc.can) #for m in cl: - # print "S:",hex(m[0]), str(m[2]).encode("hex") + # print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) can_send_many(cl) rk.keep_time() diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile index b140aa5f8..1909a7bb9 100644 --- a/selfdrive/can/Makefile +++ b/selfdrive/can/Makefile @@ -16,6 +16,7 @@ WARN_FLAGS = -Werror=implicit-function-declaration \ CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) +LDFLAGS = ifeq ($(UNAME_S),Darwin) ZMQ_LIBS = -L/usr/local/lib -lzmq @@ -28,18 +29,24 @@ else ifeq ($(UNAME_M),x86_64) else ifeq ($(UNAME_M),aarch64) ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a - CXXFLAGS += -lgnustl_shared + LDFLAGS += -lgnustl_shared endif +OBJDIR = obj + OPENDBC_PATH := $(shell python -c 'import opendbc; print opendbc.DBC_PATH') DBC_SOURCES := $(wildcard $(OPENDBC_PATH)/*.dbc) +DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES)) DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES)) +.SECONDARY: $(DBC_CCS) + +LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o CWD := $(shell pwd) .PHONY: all -all: libdbc.so +all: $(OBJDIR) libdbc.so include ../common/cereal.mk @@ -49,22 +56,45 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h ../../cereal/gen/cpp/log.capnp.h: cd ../../cereal && make -libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS) +libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS) + @echo "[ LINK ] $@" $(CXX) -fPIC -shared -o '$@' $^ \ - -I. \ - -I../.. \ - $(CXXFLAGS) \ - $(ZMQ_FLAGS) \ - $(ZMQ_LIBS) \ - $(CEREAL_CXXFLAGS) \ - $(CEREAL_LIBS) + -I. -I../.. \ + $(CXXFLAGS) \ + $(LDFLAGS) \ + $(ZMQ_FLAGS) \ + $(ZMQ_LIBS) \ + $(CEREAL_CXXFLAGS) \ + $(CEREAL_LIBS) -dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc - PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@' +$(OBJDIR)/%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) -fPIC -c -o '$@' $^ \ + -I. -I../.. \ + $(CXXFLAGS) \ + $(ZMQ_FLAGS) \ + $(CEREAL_CXXFLAGS) \ -.PHONY: clean +$(OBJDIR)/%.o: dbc_out/%.cc + @echo "[ CXX ] $@" + $(CXX) -fPIC -c -o '$@' $^ \ + -I. -I../.. \ + $(CXXFLAGS) \ + $(ZMQ_FLAGS) \ + $(CEREAL_CXXFLAGS) \ + +dbc_out/%.cc: process_dbc.py dbc_template.cc $(OPENDBC_PATH)/%.dbc + @echo "[ DBC GEN ] $@" + @echo "Missing prereq $?" + PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py $(OPENDBC_PATH) dbc_out + +$(OBJDIR): + mkdir -p $@ + +.PHONY: clean $(OBJDIR) clean: rm -rf libdbc.so* rm -f dbc_out/*.cc rm -f dbcs.txt rm -f dbcs.csv + rm -rf $(OBJDIR)/* diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index cc669e60c..7f3c8d285 100644 --- a/selfdrive/can/packer.py +++ b/selfdrive/can/packer.py @@ -1,3 +1,4 @@ +import six import struct from selfdrive.can.libdbc_py import libdbc, ffi @@ -20,7 +21,7 @@ class CANPacker(object): def pack(self, addr, values, counter): values_thing = [] - for name, value in values.iteritems(): + for name, value in six.iteritems(values): if name not in self.sig_names: self.sig_names[name] = ffi.new("char[]", name) @@ -63,5 +64,5 @@ if __name__ == "__main__": #s = cp.pack_bytes(0xe4, { # "STEER_TORQUE": -2, #}) - print [hex(ord(v)) for v in s[1]] + print([hex(ord(v)) for v in s[1]]) print(s[1].encode("hex")) diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index 8960953ed..83d0f6623 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -1,3 +1,4 @@ +import six import time from collections import defaultdict import numbers @@ -58,7 +59,7 @@ class CANParser(object): { 'address': msg_address, 'check_frequency': freq, - } for msg_address, freq in message_options.iteritems()]) + } for msg_address, freq in six.iteritems(message_options)]) self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c, len(signal_options_c), signal_options_c, sendcan, tcp_addr) @@ -68,7 +69,7 @@ class CANParser(object): value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL) self.can_values = ffi.new("SignalValue[%d]" % value_count) self.update_vl(0) - # print "===" + # print("===") def update_vl(self, sec): @@ -77,12 +78,12 @@ class CANParser(object): self.can_valid = self.p_can_valid[0] - # print can_values_len + # print(can_values_len) ret = set() for i in xrange(can_values_len): cv = self.can_values[i] address = cv.address - # print hex(cv.address), ffi.string(cv.name) + # print("{0} {1}".format(hex(cv.address), ffi.string(cv.name))) name = ffi.string(cv.name) self.vl[address][name] = cv.value self.ts[address][name] = cv.ts @@ -240,11 +241,11 @@ if __name__ == "__main__": cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0) - # print cp.vl + # print(cp.vl) while True: cp.update(int(sec_since_boot()*1e9), True) - # print cp.vl + # print(cp.vl) print(cp.ts) print(cp.can_valid) time.sleep(0.01) diff --git a/selfdrive/can/plant_can_parser.py b/selfdrive/can/plant_can_parser.py index 44939747c..e58906d7a 100644 --- a/selfdrive/can/plant_can_parser.py +++ b/selfdrive/can/plant_can_parser.py @@ -78,7 +78,7 @@ class CANParser(object): msg_vl = fix(ck_portion, msg) # compare recalculated vs received checksum if msg_vl != cdat: - print "CHECKSUM FAIL: " + hex(msg) + print("CHECKSUM FAIL: {0}".format(hex(msg))) self.ck[msg] = False self.ok[msg] = False # counter check @@ -87,13 +87,13 @@ class CANParser(object): cn = out["COUNTER"] # check counter validity if it's a relevant message if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys(): - #print hex(msg), "FAILED COUNTER!" + #print("FAILED COUNTER: {0}".format(hex(msg)() self.cn_vl[msg] += 1 # counter check failed else: self.cn_vl[msg] -= 1 # counter check passed # message status is invalid if we received too many wrong counter values if self.cn_vl[msg] >= cn_vl_max: - print "COUNTER WRONG: " + hex(msg) + print("COUNTER WRONG: {0}".format(hex(msg))) self.ok[msg] = False # update msg time stamps and counter value @@ -118,7 +118,7 @@ class CANParser(object): self.can_valid = True if False in self.ok.values(): - #print "CAN INVALID!" + #print("CAN INVALID!") self.can_valid = False return msgs_upd diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py index 810947c8e..31b131596 100755 --- a/selfdrive/can/process_dbc.py +++ b/selfdrive/can/process_dbc.py @@ -1,68 +1,89 @@ #!/usr/bin/env python import os +import glob import sys - +import six import jinja2 from collections import Counter from common.dbc import dbc -if len(sys.argv) != 3: - print "usage: %s dbc_path struct_path" % (sys.argv[0],) - sys.exit(0) +def main(): + if len(sys.argv) != 3: + print "usage: %s dbc_directory output_directory" % (sys.argv[0],) + sys.exit(0) -dbc_fn = sys.argv[1] -out_fn = sys.argv[2] + dbc_dir = sys.argv[1] + out_dir = sys.argv[2] -template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc") + template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc") + template_mtime = os.path.getmtime(template_fn) -can_dbc = dbc(dbc_fn) + this_file_mtime = os.path.getmtime(__file__) -with open(template_fn, "r") as template_f: - template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) + with open(template_fn, "r") as template_f: + template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) -msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first - for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs] + for dbc_path in glob.iglob(os.path.join(dbc_dir, "*.dbc")): + dbc_mtime = os.path.getmtime(dbc_path) + dbc_fn = os.path.split(dbc_path)[1] + dbc_name = os.path.splitext(dbc_fn)[0] + can_dbc = dbc(dbc_path) + out_fn = os.path.join(os.path.dirname(__file__), out_dir, dbc_name + ".cc") + if os.path.exists(out_fn): + out_mtime = os.path.getmtime(out_fn) + else: + out_mtime = 0 -def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates -def_vals = [(address, sig) for address, sig in sorted(def_vals.iteritems())] + if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime: + continue #skip output is newer than template and dbc -if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"): - checksum_type = "honda" - checksum_size = 4 -elif can_dbc.name.startswith("toyota") or can_dbc.name.startswith("lexus"): - checksum_type = "toyota" - checksum_size = 8 -else: - checksum_type = None + msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first + for address, ((msg_name, msg_size), msg_sigs) in sorted(six.iteritems(can_dbc.msgs)) if msg_sigs] -for address, msg_name, msg_size, sigs in msgs: - for sig in sigs: - if checksum_type is not None and sig.name == "CHECKSUM": - if sig.size != checksum_size: - sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name)) - if checksum_type == "honda" and sig.start_bit % 8 != 3: - sys.exit("CHECKSUM starts at wrong bit %s" % msg_name) - if checksum_type == "toyota" and sig.start_bit % 8 != 7: - sys.exit("CHECKSUM starts at wrong bit %s" % msg_name) - if checksum_type == "honda" and sig.name == "COUNTER": - if sig.size != 2: - sys.exit("COUNTER is not 2 bits longs %s" % msg_name) - if sig.start_bit % 8 != 5: - sys.exit("COUNTER starts at wrong bit %s" % msg_name) - if address in [0x200, 0x201]: - if sig.name == "COUNTER_PEDAL" and sig.size != 4: - sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name) - if sig.name == "CHECKSUM_PEDAL" and sig.size != 8: - sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name) + def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates + def_vals = [(address, sig) for address, sig in sorted(six.iteritems(def_vals))] -# Fail on duplicate message names -c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs]) -for name, count in c.items(): - if count > 1: - sys.exit("Duplicate message name in DBC file %s" % name) + if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"): + checksum_type = "honda" + checksum_size = 4 + elif can_dbc.name.startswith("toyota") or can_dbc.name.startswith("lexus"): + checksum_type = "toyota" + checksum_size = 8 + else: + checksum_type = None -parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len) + for address, msg_name, msg_size, sigs in msgs: + for sig in sigs: + if checksum_type is not None and sig.name == "CHECKSUM": + if sig.size != checksum_size: + sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name)) + if checksum_type == "honda" and sig.start_bit % 8 != 3: + sys.exit("CHECKSUM starts at wrong bit %s" % msg_name) + if checksum_type == "toyota" and sig.start_bit % 8 != 7: + sys.exit("CHECKSUM starts at wrong bit %s" % msg_name) + if checksum_type == "honda" and sig.name == "COUNTER": + if sig.size != 2: + sys.exit("COUNTER is not 2 bits longs %s" % msg_name) + if sig.start_bit % 8 != 5: + sys.exit("COUNTER starts at wrong bit %s" % msg_name) + if address in [0x200, 0x201]: + if sig.name == "COUNTER_PEDAL" and sig.size != 4: + sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name) + if sig.name == "CHECKSUM_PEDAL" and sig.size != 8: + sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name) -with open(out_fn, "w") as out_f: - out_f.write(parser_code) + # Fail on duplicate message names + c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs]) + for name, count in c.items(): + if count > 1: + sys.exit("Duplicate message name in DBC file %s" % name) + + parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len) + + + with open(out_fn, "w") as out_f: + out_f.write(parser_code) + +if __name__ == '__main__': + main() diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 83c2ecc32..7db36634c 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -2,9 +2,9 @@ from cereal import car from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ - create_wheel_buttons, create_lkas_heartbit, \ + create_wheel_buttons, \ create_chimes -from selfdrive.car.chrysler.values import ECU +from selfdrive.car.chrysler.values import ECU, CAR from selfdrive.can.packer import CANPacker AudibleAlert = car.CarControl.HUDControl.AudibleAlert @@ -29,6 +29,7 @@ class CarController(object): self.car_fingerprint = car_fingerprint self.alert_active = False self.send_chime = False + self.gone_fast_yet = False self.fake_ecus = set() if enable_camera: @@ -39,8 +40,8 @@ class CarController(object): def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert): - # this seems needed to avoid steering faults and to force the sync with the EPS counter + frame = CS.lkas_counter if self.prev_frame == frame: return @@ -51,6 +52,11 @@ class CarController(object): CS.steer_torque_motor, SteerLimitParams) moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message + if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit + self.gone_fast_yet = True + elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): + if CS.v_ego < (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 if not lkas_active: @@ -76,18 +82,17 @@ class CarController(object): new_msg = create_wheel_buttons(self.ccframe) can_sends.append(new_msg) + # LKAS_HEARTBIT is forwarded by Panda so no need to send it here. # frame is 100Hz (0.01s period) - if (self.ccframe % 10 == 0): # 0.1s period - new_msg = create_lkas_heartbit(self.car_fingerprint) - can_sends.append(new_msg) - if (self.ccframe % 25 == 0): # 0.25s period - new_msg = create_lkas_hud(self.packer, CS.gear_shifter, lkas_active, hud_alert, self.car_fingerprint, - self.hud_count) - can_sends.append(new_msg) - self.hud_count += 1 + if (CS.lkas_car_model != -1): + new_msg = create_lkas_hud( + self.packer, CS.gear_shifter, lkas_active, hud_alert, + self.hud_count, CS.lkas_car_model) + can_sends.append(new_msg) + self.hud_count += 1 - new_msg = create_lkas_command(self.packer, int(apply_steer), frame) + new_msg = create_lkas_command(self.packer, int(apply_steer), self.gone_fast_yet, frame) can_sends.append(new_msg) self.ccframe += 1 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index a6344027d..fd3c081f3 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -63,6 +63,18 @@ def get_can_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) +def get_camera_parser(CP): + signals = [ + # sig_name, sig_address, default + # TODO read in all the other values + ("COUNTER", "LKAS_COMMAND", -1), + ("CAR_MODEL", "LKAS_HUD", -1), + ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1) + ] + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) + class CarState(object): def __init__(self, CP): @@ -85,7 +97,7 @@ class CarState(object): self.v_ego = 0.0 - def update(self, cp): + def update(self, cp, cp_cam): # copy can_valid self.can_valid = cp.can_valid @@ -142,3 +154,7 @@ class CarState(object): self.pcm_acc_status = self.main_on self.generic_toggle = 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'] diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index e17ee3623..f803e6094 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -1,18 +1,9 @@ from cereal import car -from selfdrive.car.chrysler.values import CAR VisualAlert = car.CarControl.HUDControl.VisualAlert AudibleAlert = car.CarControl.HUDControl.AudibleAlert -MODEL_TO_CONSTANT = { - CAR.PACIFICA_2017_HYBRID: 0, - CAR.PACIFICA_2018: 0x64, - CAR.PACIFICA_2018_HYBRID: 0xa8, - CAR.PACIFICA_2019_HYBRID: 0x68, - CAR.JEEP_CHEROKEE: 0xa4, - } - def calc_checksum(data): """This function does not want the checksum byte in the input data. @@ -21,8 +12,8 @@ def calc_checksum(data): end_index = len(data) index = 0 checksum = 0xFF - temp_chk = 0; - bit_sum = 0; + temp_chk = 0 + bit_sum = 0 if(end_index <= index): return False for index in range(0, end_index): @@ -31,7 +22,7 @@ def calc_checksum(data): iterate = 8 while(iterate > 0): iterate -= 1 - bit_sum = curr & shift; + bit_sum = curr & shift temp_chk = checksum & 0x80 if (bit_sum != 0): bit_sum = 0x1C @@ -53,28 +44,20 @@ def calc_checksum(data): def make_can_msg(addr, dat): return [addr, 0, dat, 0] -def create_lkas_heartbit(car_fingerprint): - # LKAS_HEARTBIT (729) Lane-keeping heartbeat. - msg = '0000000820'.decode('hex') # 2017 - return make_can_msg(0x2d9, msg) -def create_lkas_hud(packer, gear, lkas_active, hud_alert, car_fingerprint, hud_count): +def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model): # LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed. if hud_alert == VisualAlert.steerRequired: msg = '0000000300000000'.decode('hex') return make_can_msg(0x2a6, msg) - color = 0 # default values are for park or neutral - lines = 0 + color = 1 # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019 + lines = 1 alerts = 0 - if hud_count < (3 *4): # first 3 seconds, 4Hz - lines = 1 + if hud_count < (1 *4): # first 3 seconds, 4Hz alerts = 1 - elif hud_count < (6 * 4): # next 3 seconds, 4Hz - lines = 1 - alerts = 0 # CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID # had color = 1 and lines = 1 but trying 2017 hybrid style for now. if gear in ('drive', 'reverse', 'low'): @@ -87,7 +70,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, car_fingerprint, hud_c values = { "LKAS_ICON_COLOR": color, # byte 0, last 2 bits - "CAR_MODEL": MODEL_TO_CONSTANT[car_fingerprint], # byte 1 + "CAR_MODEL": lkas_car_model, # byte 1 "LKAS_LANE_LINES": lines, # byte 2, last 4 bits "LKAS_ALERTS": alerts, # byte 3, last 4 bits } @@ -95,11 +78,11 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, car_fingerprint, hud_c return packer.make_can_msg("LKAS_HUD", 0, values) # 0x2a6 -def create_lkas_command(packer, apply_steer, frame): - # LKAS_COMMAND (658) Lane-keeping signal to turn the wheel. +def create_lkas_command(packer, apply_steer, moving_fast, frame): + # LKAS_COMMAND 0x292 (658) Lane-keeping signal to turn the wheel. values = { "LKAS_STEERING_TORQUE": apply_steer, - "LKAS_HIGH_TORQUE": 1, + "LKAS_HIGH_TORQUE": int(moving_fast), "COUNTER": frame % 0x10, } diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/chryslercan_test.py index 2edfff9f2..4bcb346ba 100644 --- a/selfdrive/car/chrysler/chryslercan_test.py +++ b/selfdrive/car/chrysler/chryslercan_test.py @@ -1,5 +1,4 @@ -import chryslercan -from values import CAR +from selfdrive.car.chrysler import chryslercan from selfdrive.can.packer import CANPacker from cereal import car @@ -15,33 +14,47 @@ class TestChryslerCan(unittest.TestCase): self.assertEqual(0x75, chryslercan.calc_checksum([0x01, 0x20])) self.assertEqual(0xcc, chryslercan.calc_checksum([0x14, 0, 0, 0, 0x20])) - def test_heartbit(self): - self.assertEqual( - [0x2d9, 0, '0000000820'.decode('hex'), 0], - chryslercan.create_lkas_heartbit(CAR.PACIFICA_2017_HYBRID)) - def test_hud(self): packer = CANPacker('chrysler_pacifica_2017_hybrid') self.assertEqual( - [0x2a6, 0, '0000010100000000'.decode('hex'), 0], - chryslercan.create_lkas_hud(packer, - 'park', False, False, CAR.PACIFICA_2017_HYBRID, 1)) + [0x2a6, 0, '0100010100000000'.decode('hex'), 0], + chryslercan.create_lkas_hud( + packer, + 'park', False, False, 1, 0)) self.assertEqual( - [0x2a6, 0, '0000010000000000'.decode('hex'), 0], - chryslercan.create_lkas_hud(packer, - 'park', False, False, CAR.PACIFICA_2017_HYBRID, 5*4)) + [0x2a6, 0, '0100010000000000'.decode('hex'), 0], + chryslercan.create_lkas_hud( + packer, + 'park', False, False, 5*4, 0)) self.assertEqual( - [0x2a6, 0, '0000000000000000'.decode('hex'), 0], - chryslercan.create_lkas_hud(packer, - 'park', False, False, CAR.PACIFICA_2017_HYBRID, 99999)) + [0x2a6, 0, '0100010000000000'.decode('hex'), 0], + chryslercan.create_lkas_hud( + packer, + 'park', False, False, 99999, 0)) self.assertEqual( [0x2a6, 0, '0200060000000000'.decode('hex'), 0], - chryslercan.create_lkas_hud(packer, - 'drive', True, False, CAR.PACIFICA_2017_HYBRID, 99999)) + chryslercan.create_lkas_hud( + packer, + 'drive', True, False, 99999, 0)) self.assertEqual( [0x2a6, 0, '0264060000000000'.decode('hex'), 0], - chryslercan.create_lkas_hud(packer, - 'drive', True, False, CAR.PACIFICA_2018, 99999)) + chryslercan.create_lkas_hud( + packer, + 'drive', True, False, 99999, 0x64)) + + def test_command(self): + packer = CANPacker('chrysler_pacifica_2017_hybrid') + self.assertEqual( + [0x292, 0, '140000001086'.decode('hex'), 0], + chryslercan.create_lkas_command( + packer, + 0, True, 1)) + self.assertEqual( + [0x292, 0, '040000008083'.decode('hex'), 0], + chryslercan.create_lkas_command( + packer, + 0, False, 8)) + if __name__ == '__main__': unittest.main() diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ac797616b..e0551ef0c 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -4,7 +4,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.chrysler.carstate import CarState, get_can_parser +from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR try: @@ -27,8 +27,8 @@ class CarInterface(object): # *** init the major players *** self.CS = CarState(CP) - self.cp = get_can_parser(CP) + self.cp_cam = get_camera_parser(CP) # sending if read only is False if sendcan is not None: @@ -79,7 +79,7 @@ class CarInterface(object): ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 - if candidate == CAR.JEEP_CHEROKEE: + if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): ret.wheelbase = 2.91 # in meters ret.steerRatio = 12.7 ret.steerActuatorDelay = 0.2 # in seconds @@ -91,6 +91,9 @@ class CarInterface(object): ret.minSteerSpeed = 3.8 # m/s ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this + if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): + ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + # TODO allow 2019 cars to steer down to 13 m/s if already engaged. centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for @@ -119,7 +122,7 @@ class CarInterface(object): ret.brakeMaxV = [1., 0.8] ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) - print "ECU Camera Simulated: ", ret.enableCamera + print("ECU Camera Simulated: {0}".format(ret.enableCamera)) ret.openpilotLongitudinalControl = False ret.steerLimitAlert = True @@ -137,10 +140,9 @@ class CarInterface(object): def update(self, c): # ******************* do can recv ******************* canMonoTimes = [] - self.cp.update(int(sec_since_boot() * 1e9), False) - - self.CS.update(self.cp) + self.cp_cam.update(int(sec_since_boot() * 1e9), False) + self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() @@ -208,6 +210,8 @@ class CarInterface(object): 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 = [] diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 60eaa4566..54bd1b314 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -25,29 +25,29 @@ def _create_radard_can_parser(): # The factor and offset are applied by the dbc parsing library, so the # default values should be after the factor/offset are applied. - signals = zip(['LONG_DIST'] * msg_n + + signals = list(zip(['LONG_DIST'] * msg_n + ['LAT_DIST'] * msg_n + ['REL_SPEED'] * msg_n, RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST RADAR_MSGS_D, # REL_SPEED [0] * msg_n + # LONG_DIST [-1000] * msg_n + # LAT_DIST - [-146.278] * msg_n) # REL_SPEED set to 0, factor/offset to this + [-146.278] * msg_n)) # REL_SPEED set to 0, factor/offset to this # TODO what are the checks actually used for? # honda only checks the last message, # toyota checks all the messages. Which do we want? - checks = zip(RADAR_MSGS_C + + checks = list(zip(RADAR_MSGS_C + RADAR_MSGS_D, [20]*msg_n + # 20Hz (0.05s) - [20]*msg_n) # 20Hz (0.05s) + [20]*msg_n)) # 20Hz (0.05s) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) def _address_to_track(address): if address in RADAR_MSGS_C: - return (address - RADAR_MSGS_C[0]) / 2 + return (address - RADAR_MSGS_C[0]) // 2 if address in RADAR_MSGS_D: - return (address - RADAR_MSGS_D[0]) / 2 + return (address - RADAR_MSGS_D[0]) // 2 raise ValueError("radar received unexpected address %d" % address) class RadarInterface(object): @@ -104,4 +104,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") # clear screen - print ret + print(ret) diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 6e3a24b8f..4636b89a3 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -6,6 +6,7 @@ class CAR: PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" PACIFICA_2018 = "CHRYSLER PACIFICA 2018" JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017. + JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # Unique can messages: # Only the hybrids have 270: 8 @@ -24,11 +25,9 @@ FINGERPRINTS = { ], CAR.PACIFICA_2018: [ {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 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, 416: 7, 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: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 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, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 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, 1098: 8, 1100: 8}, - ], CAR.PACIFICA_2018_HYBRID: [ - {68: 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: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 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, 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, 969: 4, 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}, - {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: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 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, 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, 969: 4, 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}, + {68: 8, 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: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 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, 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, 969: 4, 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}, ], CAR.PACIFICA_2019_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, 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, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 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, 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, 1538: 8}, @@ -42,6 +41,8 @@ FINGERPRINTS = { {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 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, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 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, 1062: 8, 1098: 8, 1100: 8}, # Jeep Grand Cherokee 2017 Trailhawk {257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 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, 1062: 8, 1098: 8, 1100: 8}, + ], + CAR.JEEP_CHEROKEE_2019: [ # Jeep Grand Cherokee 2019 from Switzerland # 530: 8 is so far only in this Jeep. {55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 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, 1062: 8, 1098: 8, 1100: 8}, @@ -65,6 +66,9 @@ DBC = { CAR.JEEP_CHEROKEE: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' + CAR.JEEP_CHEROKEE_2019: dbc_dict( # Same DBC file works. + 'chrysler_pacifica_2017_hybrid', # 'pt' + 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' } STEER_THRESHOLD = 120 diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 0e29ea626..25e4d2b9c 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -6,7 +6,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.ford.carstate import CarState, get_can_parser -from selfdrive.car.ford.fordcan import MAX_ANGLE +from selfdrive.car.ford.values import MAX_ANGLE try: from selfdrive.car.ford.carcontroller import CarController diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 2acea9d70..c42bc3e8f 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -14,10 +14,10 @@ RADAR_MSGS = range(0x500, 0x540) def _create_radard_can_parser(): dbc_f = 'ford_fusion_2018_adas.dbc' msg_n = len(RADAR_MSGS) - signals = zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, - RADAR_MSGS * 3, - [0] * msg_n + [0] * msg_n + [0] * msg_n) - checks = zip(RADAR_MSGS, [20]*msg_n) + signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, + RADAR_MSGS * 3, + [0] * msg_n + [0] * msg_n + [0] * msg_n)) + checks = list(zip(RADAR_MSGS, [20]*msg_n)) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) @@ -90,4 +90,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index b8890107f..6ab0b3750 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,12 +1,14 @@ from selfdrive.car import dbc_dict +MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault + class CAR: FUSION = "FORD FUSION 2018" FINGERPRINTS = { CAR.FUSION: [{ 71: 8, 74: 8, 75: 8, 76: 8, 90: 8, 92: 8, 93: 8, 118: 8, 119: 8, 120: 8, 125: 8, 129: 8, 130: 8, 131: 8, 132: 8, 133: 8, 145: 8, 146: 8, 357: 8, 359: 8, 360: 8, 361: 8, 376: 8, 390: 8, 391: 8, 392: 8, 394: 8, 512: 8, 514: 8, 516: 8, 531: 8, 532: 8, 534: 8, 535: 8, 560: 8, 578: 8, 604: 8, 613: 8, 673: 8, 827: 8, 848: 8, 934: 8, 935: 8, 936: 8, 947: 8, 963: 8, 970: 8, 972: 8, 973: 8, 984: 8, 992: 8, 994: 8, 997: 8, 998: 8, 1003: 8, 1034: 8, 1045: 8, 1046: 8, 1053: 8, 1054: 8, 1058: 8, 1059: 8, 1068: 8, 1072: 8, 1073: 8, 1082: 8, 1107: 8, 1108: 8, 1109: 8, 1110: 8, 1200: 8, 1427: 8, 1430: 8, 1438: 8, 1459: 8 - }], + }], } DBC = { diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b242bc57b..67b240536 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -104,7 +104,7 @@ class CarController(object): apply_steer = 0 self.apply_steer_last = apply_steer - idx = (frame / P.STEER_STEP) % 4 + idx = (frame // P.STEER_STEP) % 4 if self.car_fingerprint in SUPERCRUISE_CARS: can_sends += gmcan.create_steering_control_ct6(self.packer_pt, @@ -134,7 +134,7 @@ class CarController(object): # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: - idx = (frame / 4) % 4 + idx = (frame // 4) % 4 at_full_stop = enabled and CS.standstill near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) @@ -153,13 +153,13 @@ class CarController(object): tt = sec_since_boot() if frame % time_and_headlights_step == 0: - idx = (frame / time_and_headlights_step) % 4 + idx = (frame // time_and_headlights_step) % 4 can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx)) can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: - idx = (frame / speed_and_accelerometer_step) % 4 + idx = (frame // speed_and_accelerometer_step) % 4 can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx)) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 83e90c3c7..d5afffbaa 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -70,7 +70,7 @@ class CarInterface(object): # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS # kg of standard extra cargo to count for driver, gas, etc... - ret.mass = 1607 + std_cargo + ret.mass = 1607. + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 @@ -80,7 +80,7 @@ class CarInterface(object): elif candidate == CAR.MALIBU: # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS - ret.mass = 1496 + std_cargo + ret.mass = 1496. + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 @@ -89,7 +89,7 @@ class CarInterface(object): elif candidate == CAR.HOLDEN_ASTRA: # kg of standard extra cargo to count for driver, gas, etc... - ret.mass = 1363 + std_cargo + ret.mass = 1363. + std_cargo ret.wheelbase = 2.662 # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 @@ -99,7 +99,7 @@ class CarInterface(object): ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: - ret.minEnableSpeed = -1 # engage speed is decided by pcm + ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.86 @@ -118,7 +118,7 @@ class CarInterface(object): elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS - ret.mass = 1601 + std_cargo + ret.mass = 1601. + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 @@ -127,7 +127,7 @@ class CarInterface(object): elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm - ret.minEnableSpeed = -1 + ret.minEnableSpeed = -1. # kg of standard extra cargo to count for driver, gas, etc... ret.mass = 4016. * CV.LB_TO_KG + std_cargo ret.safetyModel = car.CarParams.SafetyModels.cadillac diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 6af0d3f8f..e299ca6fc 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -25,7 +25,7 @@ def create_radard_can_parser(canbus, car_fingerprint): if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): # C1A-ARS3-A by Continental radar_targets = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS) - signals = zip(['FLRRNumValidTargets', + signals = list(zip(['FLRRNumValidTargets', 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + @@ -36,7 +36,7 @@ def create_radard_can_parser(canbus, car_fingerprint): [0] * 7 + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + - [0.0] * NUM_SLOTS + [0] * NUM_SLOTS) + [0.0] * NUM_SLOTS + [0] * NUM_SLOTS)) checks = [] @@ -124,4 +124,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 08c0235fd..716758313 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -12,9 +12,9 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params brake_hyst_on = 0.02 # to activate brakes exceed this value brake_hyst_off = 0.005 # to deactivate brakes below this value - brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value + brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value - #*** histeresis logic to avoid brake blinking. go above 0.1 to trigger + #*** hysteresis logic to avoid brake blinking. go above 0.1 to trigger if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off: brake = 0. braking = brake > 0. @@ -34,7 +34,7 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): return brake, braking, brake_steady -def brake_pump_hysteresys(apply_brake, apply_brake_last, last_pump_ts): +def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts): ts = sec_since_boot() pump_on = False @@ -124,7 +124,7 @@ class CarController(object): if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep != 0 else snd_chime - #print chime, alert_id, hud_alert + #print("{0} {1} {2}".format(chime, alert_id, hud_alert)) fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, @@ -133,11 +133,13 @@ class CarController(object): # **** process the car messages **** # *** compute control surfaces *** - BRAKE_MAX = 1024/4 + BRAKE_MAX = 1024//4 if CS.CP.carFingerprint in (CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) + elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): + STEER_MAX = 0x7FFF else: STEER_MAX = 0x1000 @@ -158,7 +160,7 @@ class CarController(object): # Send dashboard UI commands. if (frame % 10) == 0: - idx = (frame/10) % 4 + idx = (frame//10) % 4 can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) if CS.CP.radarOffCan: @@ -171,8 +173,8 @@ class CarController(object): else: # Send gas and brake commands. if (frame % 2) == 0: - idx = frame / 2 - pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts) + idx = frame // 2 + pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) self.apply_brake_last = apply_brake diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 718e7ae61..b8164e8c5 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -60,18 +60,35 @@ def get_can_signals(CP): ("ENGINE_DATA", 100), ("WHEEL_SPEEDS", 50), ("STEERING_SENSORS", 100), - ("SCM_FEEDBACK", 10), - ("GEARBOX", 100), ("SEATBELT_STATUS", 10), ("CRUISE", 10), ("POWERTRAIN_DATA", 100), ("VSA_STATUS", 50), - ("SCM_BUTTONS", 25), ] + if CP.carFingerprint == CAR.ODYSSEY_CHN: + checks += [ + ("SCM_FEEDBACK", 25), + ("SCM_BUTTONS", 50), + ] + else: + checks += [ + ("SCM_FEEDBACK", 10), + ("SCM_BUTTONS", 25), + ] + + if CP.carFingerprint == CAR.CRV_HYBRID: + checks += [ + ("GEARBOX", 50), + ] + else: + checks += [ + ("GEARBOX", 100), + ] + if CP.radarOffCan: # Civic is only bosch to use the same brake message as other hondas. - if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH): + if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] checks += [("BRAKE_MODULE", 50)] signals += [("CAR_GAS", "GAS_PEDAL_2", 0), @@ -85,11 +102,17 @@ def get_can_signals(CP): ("BRAKE_ERROR_2", "STANDSTILL", 1), ("CRUISE_SPEED_PCM", "CRUISE", 0), ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)] - checks += [("CRUISE_PARAMS", 50), - ("STANDSTILL", 50)] + checks += [("STANDSTILL", 50)] - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): + if CP.carFingerprint == CAR.ODYSSEY_CHN: + checks += [("CRUISE_PARAMS", 10)] + else: + checks += [("CRUISE_PARAMS", 50)] + + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] + elif CP.carFingerprint == CAR.ODYSSEY_CHN: + signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] else: signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), ("DOOR_OPEN_FR", "DOORS_STATUS", 1), @@ -114,6 +137,10 @@ def get_can_signals(CP): elif CP.carFingerprint == CAR.PILOT: signals += [("MAIN_ON", "SCM_BUTTONS", 0), ("CAR_GAS", "GAS_PEDAL_2", 0)] + elif CP.carFingerprint == CAR.ODYSSEY_CHN: + signals += [("MAIN_ON", "SCM_BUTTONS", 0), + ("EPB_STATE", "EPB_STATUS", 0)] + checks += [("EPB_STATUS", 50)] # add gas interceptor reading if we are using it if CP.enableGasInterceptor: @@ -130,9 +157,9 @@ def get_can_parser(CP): def get_cam_can_parser(CP): signals = [] - # all hondas except CRV and RDX use 0xe4 for steering + # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering checks = [(0xe4, 100)] - if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX]: + if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: checks = [(0x194, 100)] cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2 @@ -189,9 +216,12 @@ class CarState(object): # ******************* parse out can ******************* - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): # TODO: find wheels moving bit in dbc + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] + elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: + self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 + self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'] else: self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], @@ -248,9 +278,12 @@ class CarState(object): self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] - if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): + if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] + elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: + self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 + self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] else: self.park_brake = 0 # TODO self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] @@ -260,7 +293,7 @@ class CarState(object): self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] # crv doesn't include cruise control - if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019): + if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN): self.car_gas = self.pedal_gas else: self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] @@ -273,7 +306,7 @@ class CarState(object): if self.CP.radarOffCan: self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) - if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH): + if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID): self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ (self.brake_switch and self.brake_switch_prev and \ diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 435360d65..8f369feca 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -65,8 +65,8 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): 'CRUISE_SPEED': hud.v_cruise, 'ENABLE_MINI_CAR': hud.mini_car, 'HUD_LEAD': hud.car, - 'SET_ME_X03': 0x03, - 'SET_ME_X03_2': 0x03, + 'SET_ME_X03': 0x01 if car_fingerprint == CAR.ODYSSEY_CHN else 0x03, + 'SET_ME_X03_2': 0x02 if car_fingerprint == CAR.ODYSSEY_CHN else 0x03, 'SET_ME_X01': 0x01, } commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3acd2c741..6c4b4a767 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -196,10 +196,10 @@ class CarInterface(object): tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] if is_fw_modified: - tire_stiffness_factor = 0.9 ret.steerKf = 0.00004 + + ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiBP = [0., 35.] @@ -260,6 +260,20 @@ class CarInterface(object): ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.CRV_HYBRID: + stop_and_go = True + ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.mass = 1667. + std_cargo # mean of 4 models in kg + ret.wheelbase = 2.66 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 16.0 # 12.3 is spec end-to-end + tire_stiffness_factor = 0.677 + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ACURA_RDX: stop_and_go = False ret.mass = 3935 * CV.LB_TO_KG + std_cargo @@ -286,6 +300,19 @@ class CarInterface(object): ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ODYSSEY_CHN: + stop_and_go = False + ret.mass = 1849.2 + std_cargo # mean of 4 models in kg + ret.wheelbase = 2.90 # spec + ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY + ret.steerRatio = 14.35 # from CAR.ODYSSEY + tire_stiffness_factor = 0.82 # from CAR.ODYSSEY + ret.steerKpV, ret.steerKiV = [[0.45], [0.135]] + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] + elif candidate in (CAR.PILOT, CAR.PILOT_2019): stop_and_go = False ret.mass = 4303 * CV.LB_TO_KG + std_cargo diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 7d30265c2..9977e763a 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -12,12 +12,12 @@ import selfdrive.messaging as messaging def _create_nidec_can_parser(): dbc_f = 'acura_ilx_2016_nidec.dbc' radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446) - signals = zip(['RADAR_STATE'] + + signals = list(zip(['RADAR_STATE'] + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + ['REL_SPEED'] * 16, [0x400] + radar_messages[1:] * 4, - [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) - checks = zip([0x445], [20]) + [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)) + checks = list(zip([0x445], [20])) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) @@ -101,4 +101,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 56302551f..22fb53d7c 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -65,7 +65,9 @@ class CAR: ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS" CRV = "HONDA CR-V 2016 TOURING" CRV_5G = "HONDA CR-V 2017 EX" + CRV_HYBRID = "HONDA CR-V 2019 HYBRID" ODYSSEY = "HONDA ODYSSEY 2018 EX-L" + ODYSSEY_CHN = "HONDA ODYSSEY 2019 EXCLUSIVE CHN" ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS" PILOT = "HONDA PILOT 2017 TOURING" PILOT_2019 = "HONDA PILOT 2019 ELITE" @@ -101,6 +103,9 @@ FINGERPRINTS = { CAR.CRV_5G: [{ 57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 4, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5 }], + CAR.CRV_HYBRID: [{ + 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8 + }], # 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L) CAR.ODYSSEY: [{ 57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5 @@ -109,16 +114,20 @@ FINGERPRINTS = { { 57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5 }], + CAR.ODYSSEY_CHN: [{ + 57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8 + }], # 2017 Pilot Touring AND 2016 Pilot EX-L w/ Added Comma Pedal Support (512L & 513L) CAR.PILOT: [{ 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5 }], + # this fingerprint also includes the Passport 2019 CAR.PILOT_2019: [{ - 57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5 + 57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5 }, # 2019 Pilot EX-L { - 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5 + 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5 }], # Ridgeline w/ Added Comma Pedal Support (512L & 513L) CAR.RIDGELINE: [{ @@ -140,7 +149,9 @@ DBC = { CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None), + CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), + CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can', 'acura_ilx_2016_nidec'), CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), @@ -156,7 +167,9 @@ STEER_THRESHOLD = { CAR.CIVIC_BOSCH: 1200, CAR.CRV: 1200, CAR.CRV_5G: 1200, + CAR.CRV_HYBRID: 1200, CAR.ODYSSEY: 1200, + CAR.ODYSSEY_CHN: 1200, CAR.PILOT: 1200, CAR.PILOT_2019: 1200, CAR.RIDGELINE: 1200, @@ -172,11 +185,13 @@ SPEED_FACTOR = { CAR.CIVIC_BOSCH: 1., CAR.CRV: 1.025, CAR.CRV_5G: 1.025, + CAR.CRV_HYBRID: 1.025, CAR.ODYSSEY: 1., + CAR.ODYSSEY_CHN: 1., CAR.PILOT: 1., CAR.PILOT_2019: 1., CAR.RIDGELINE: 1., } # TODO: get these from dbc file -HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G] +HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index d7fc2bd39..618656558 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -68,13 +68,13 @@ class CarInterface(object): rotationalInertia_civic = 2500 tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + tire_stiffness_factor = 1. ret.steerActuatorDelay = 0.1 # Default delay - tire_stiffness_factor = 1. + ret.steerRateCost = 0.5 if candidate == CAR.SANTA_FE: ret.steerKf = 0.00005 - ret.steerRateCost = 0.5 ret.mass = 3982 * CV.LB_TO_KG + std_cargo ret.wheelbase = 2.766 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 96159fd87..3e458df57 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -21,4 +21,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py index ec042d179..a99494b1a 100755 --- a/selfdrive/car/mock/radar_interface.py +++ b/selfdrive/car/mock/radar_interface.py @@ -13,7 +13,6 @@ class RadarInterface(object): ret = car.RadarState.new_message() time.sleep(0.05) # radard runs on RI updates - return ret if __name__ == "__main__": @@ -21,4 +20,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py new file mode 100644 index 000000000..33a0fde86 --- /dev/null +++ b/selfdrive/car/subaru/carcontroller.py @@ -0,0 +1,76 @@ +#from common.numpy_fast import clip +from common.realtime import sec_since_boot +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.subaru import subarucan +from selfdrive.car.subaru.values import CAR, DBC +from selfdrive.can.packer import CANPacker + + +class CarControllerParams(): + def __init__(self, car_fingerprint): + self.STEER_MAX = 2047 # max_steer 4095 + self.STEER_STEP = 2 # how often we update the steer cmd + self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max + self.STEER_DELTA_DOWN = 70 # torque decrease per refresh + if car_fingerprint == CAR.IMPREZA: + self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily + self.STEER_DRIVER_FACTOR = 1 # from dbc + + + +class CarController(object): + def __init__(self, car_fingerprint): + self.start_time = sec_since_boot() + self.lkas_active = False + self.steer_idx = 0 + self.apply_steer_last = 0 + self.car_fingerprint = car_fingerprint + self.es_distance_cnt = -1 + self.es_lkas_cnt = -1 + + # Setup detection helper. Routes commands to + # an appropriate CAN bus number. + self.params = CarControllerParams(car_fingerprint) + print(DBC) + self.packer = CANPacker(DBC[car_fingerprint]['pt']) + + def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert): + """ Controls thread """ + + P = self.params + + # Send CAN commands. + can_sends = [] + + ### STEER ### + + if (frame % P.STEER_STEP) == 0: + + final_steer = actuators.steer if enabled else 0. + apply_steer = int(round(final_steer * P.STEER_MAX)) + + # limits due to driver torque + + apply_steer = int(round(apply_steer)) + apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) + + lkas_enabled = enabled and not CS.steer_not_allowed + + if not lkas_enabled: + apply_steer = 0 + + can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP)) + + self.apply_steer_last = apply_steer + + if self.es_distance_cnt != CS.es_distance_msg["Counter"]: + can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd)) + self.es_distance_cnt = CS.es_distance_msg["Counter"] + + if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert)) + self.es_lkas_cnt = CS.es_lkas_msg["Counter"] + + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index e12f5f957..38d2fe479 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,3 +1,4 @@ +import copy import numpy as np from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV @@ -32,12 +33,49 @@ def get_powertrain_can_parser(CP): ("Dashlights", 10), ("CruiseControl", 20), ("Wheel_Speeds", 50), - ("Steering_Torque", 100), + ("Steering_Torque", 50), ("BodyInfo", 10), ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) +def get_camera_can_parser(CP): + signals = [ + ("Cruise_Set_Speed", "ES_DashStatus", 0), + + ("Counter", "ES_Distance", 0), + ("Signal1", "ES_Distance", 0), + ("Signal2", "ES_Distance", 0), + ("Main", "ES_Distance", 0), + ("Signal3", "ES_Distance", 0), + + ("Checksum", "ES_LKAS_State", 0), + ("Counter", "ES_LKAS_State", 0), + ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), + ("Empty_Box", "ES_LKAS_State", 0), + ("Signal1", "ES_LKAS_State", 0), + ("LKAS_ACTIVE", "ES_LKAS_State", 0), + ("Signal2", "ES_LKAS_State", 0), + ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), + ("LKAS_ENABLE_3", "ES_LKAS_State", 0), + ("Signal3", "ES_LKAS_State", 0), + ("LKAS_ENABLE_2", "ES_LKAS_State", 0), + ("Signal4", "ES_LKAS_State", 0), + ("FCW_Cont_Beep", "ES_LKAS_State", 0), + ("FCW_Repeated_Beep", "ES_LKAS_State", 0), + ("Throttle_Management_Activated", "ES_LKAS_State", 0), + ("Traffic_light_Ahead", "ES_LKAS_State", 0), + ("Right_Depart", "ES_LKAS_State", 0), + ("Signal5", "ES_LKAS_State", 0), + + ] + + checks = [ + ("ES_DashStatus", 10), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) + class CarState(object): def __init__(self, CP): # initialize can parser @@ -60,9 +98,11 @@ class CarState(object): K=np.matrix([[0.12287673], [0.29666309]])) self.v_ego = 0. - def update(self, cp): + def update(self, cp, cp_cam): + + self.can_valid = cp.can_valid + self.cam_can_valid = cp_cam.can_valid - self.can_valid = True self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal'] self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal'] self.user_gas_pressed = self.pedal_gas > 0 @@ -74,6 +114,8 @@ class CarState(object): self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS + self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.MPH_TO_KPH + v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. # 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 @@ -101,4 +143,5 @@ class CarState(object): cp.vl["BodyInfo"]['DOOR_OPEN_FR'], cp.vl["BodyInfo"]['DOOR_OPEN_FL']]) - + self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"]) + self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 93a1ac62b..ef3da2f39 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.subaru.values import CAR -from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser +from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser try: from selfdrive.car.subaru.carcontroller import CarController @@ -20,11 +20,15 @@ class CarInterface(object): self.frame = 0 self.can_invalid_count = 0 self.acc_active_prev = 0 + self.gas_pressed_prev = False # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) self.pt_cp = get_powertrain_can_parser(CP) + self.cam_cp = get_camera_can_parser(CP) + + self.gas_pressed_prev = False # sending if read only is False if sendcan is not None: @@ -47,8 +51,9 @@ class CarInterface(object): ret.carFingerprint = candidate ret.safetyModel = car.CarParams.SafetyModels.subaru - ret.enableCruise = False + ret.enableCruise = True ret.steerLimitAlert = True + ret.enableCamera = True std_cargo = 136 @@ -116,7 +121,8 @@ class CarInterface(object): def update(self, c): self.pt_cp.update(int(sec_since_boot() * 1e9), False) - self.CS.update(self.pt_cp) + self.cam_cp.update(int(sec_since_boot() * 1e9), False) + self.CS.update(self.pt_cp, self.cam_cp) # create message ret = car.CarState.new_message() @@ -140,11 +146,19 @@ class CarInterface(object): ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver + ret.gas = self.CS.pedal_gas / 255. + ret.gasPressed = self.CS.user_gas_pressed + # cruise state + ret.cruiseState.enabled = bool(self.CS.acc_active) + ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) + ret.cruiseState.speedOffset = 0. + ret.leftBlinker = self.CS.left_blinker_on ret.rightBlinker = self.CS.right_blinker_on ret.seatbeltUnlatched = self.CS.seatbelt_unlatched + ret.doorOpen = self.CS.door_open buttonEvents = [] @@ -177,29 +191,31 @@ class CarInterface(object): if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if ret.doorOpen: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if self.CS.acc_active and not self.acc_active_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) if not self.CS.acc_active: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - ## handle button presses - #for b in ret.buttonEvents: - # # do enable on both accel and decel buttons - # if b.type in ["accelCruise", "decelCruise"] and not b.pressed: - # events.append(create_event('buttonEnable', [ET.ENABLE])) - # # do disable on button down - # if b.type == "cancel" and b.pressed: - # events.append(create_event('buttonCancel', [ET.USER_DISABLE])) + # disable on gas pedal rising edge + if (ret.gasPressed and not self.gas_pressed_prev): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events # update previous brake/gas pressed + self.gas_pressed_prev = ret.gasPressed self.acc_active_prev = self.CS.acc_active - # cast to reader so it can't be modified return ret.as_reader() def apply(self, c): - self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators) + self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, + c.cruiseControl.cancel, c.hudControl.visualAlert) self.frame += 1 diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py index 96159fd87..3e458df57 100644 --- a/selfdrive/car/subaru/radar_interface.py +++ b/selfdrive/car/subaru/radar_interface.py @@ -21,4 +21,4 @@ if __name__ == "__main__": while 1: ret = RI.update() print(chr(27) + "[2J") - print ret + print(ret) diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py new file mode 100644 index 000000000..5e17cd72c --- /dev/null +++ b/selfdrive/car/subaru/subarucan.py @@ -0,0 +1,54 @@ +import copy +from cereal import car +from selfdrive.car.subaru.values import CAR + +VisualAlert = car.CarControl.HUDControl.VisualAlert + +def subaru_checksum(packer, values, addr): + dat = packer.make_can_msg(addr, 0, values)[2] + dat = [ord(i) for i in dat] + return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff + +def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step): + + if car_fingerprint == CAR.IMPREZA: + #counts from 0 to 15 then back to 0 + 16 for enable bit + idx = ((frame // steer_step) % 16) + + values = { + "Counter": idx, + "LKAS_Output": apply_steer, + "LKAS_Request": 1 if apply_steer != 0 else 0, + "SET_1": 1 + } + values["Checksum"] = subaru_checksum(packer, values, 0x122) + + return packer.make_can_msg("ES_LKAS", 0, values) + +def create_steering_status(packer, car_fingerprint, apply_steer, frame, steer_step): + + if car_fingerprint == CAR.IMPREZA: + values = {} + values["Checksum"] = subaru_checksum(packer, {}, 0x322) + + return packer.make_can_msg("ES_LKAS_State", 0, values) + +def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd): + + values = copy.copy(es_distance_msg) + if pcm_cancel_cmd: + values["Main"] = 1 + + values["Checksum"] = subaru_checksum(packer, values, 545) + + return packer.make_can_msg("ES_Distance", 0, values) + +def create_es_lkas(packer, es_lkas_msg, visual_alert): + + values = copy.copy(es_lkas_msg) + if visual_alert == VisualAlert.steerRequired: + values["Keep_Hands_On_Wheel"] = 1 + + values["Checksum"] = subaru_checksum(packer, values, 802) + + return packer.make_can_msg("ES_LKAS_State", 0, values) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index cdd1bde51..d69636d32 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -125,7 +125,8 @@ class CarController(object): self.packer = CANPacker(dbc_name) def update(self, sendcan, enabled, CS, frame, actuators, - pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead): + pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, + left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** @@ -161,7 +162,7 @@ class CarController(object): self.steer_angle_enabled, self.ipas_reset_counter = \ ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) - #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active + #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) # steer angle if self.steer_angle_enabled and CS.ipas_active: @@ -198,7 +199,7 @@ class CarController(object): can_sends = [] #*** control msgs *** - #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor + #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed @@ -226,11 +227,11 @@ class CarController(object): if (frame % 2 == 0) and (CS.CP.enableGasInterceptor): # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_command(self.packer, apply_gas, frame/2)) + can_sends.append(create_gas_command(self.packer, apply_gas, frame//2)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: for addr in TARGET_IDS: - can_sends.append(create_video_target(frame/10, addr)) + can_sends.append(create_video_target(frame//10, addr)) # ui mesg is at 100Hz but we send asap if: # - there is something to display @@ -246,7 +247,7 @@ class CarController(object): send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: - can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line)) + can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus: can_sends.append(create_fcw_command(self.packer, fcw)) @@ -257,11 +258,11 @@ class CarController(object): if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera): # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: - cnt = (((frame / 5) % 7) + 1) << 5 + cnt = (((frame // 5) % 7) + 1) << 5 vl = chr(cnt) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) - cnt = ((frame/100)%0xf) + 1 + cnt = ((frame // 100) % 0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b6cb0a01..0fe0d0821 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -369,7 +369,8 @@ class CarInterface(object): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert, self.forwarding_camera, - c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible) + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, + c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 return False diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 9711f1393..054ce0b7b 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -19,12 +19,13 @@ def _create_radard_can_parser(): msg_a_n = len(RADAR_A_MSGS) msg_b_n = len(RADAR_B_MSGS) - signals = zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + - ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, - RADAR_A_MSGS * 5 + RADAR_B_MSGS, - [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n) + signals = list(zip( + ['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + + ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, + RADAR_A_MSGS * 5 + RADAR_B_MSGS, + [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)) - checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)) + checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 720c0ef60..85b8dce8d 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -89,10 +89,11 @@ def create_fcw_command(packer, fcw): return packer.make_can_msg("ACC_HUD", 0, values) -def create_ui_command(packer, steer, sound1, sound2, left_line, right_line): +def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart): values = { - "RIGHT_LINE": 1 if right_line else 2, - "LEFT_LINE": 1 if left_line else 2, + "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, + "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, + "BARRIERS" : 3 if left_lane_depart or right_lane_depart else 0, "SET_ME_X0C": 0x0c, "SET_ME_X2C": 0x2c, "SET_ME_X38": 0x38, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 2f3f44448..ce291ef3a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -86,7 +86,7 @@ FINGERPRINTS = { }, # Chinese RAV4 { - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 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, 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 @@ -97,11 +97,15 @@ FINGERPRINTS = { 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 }], 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, 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, 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 + # RX450HL { - 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, 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, 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 + }, + # 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 }], 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 @@ -122,7 +126,7 @@ FINGERPRINTS = { #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, 869: 7, 870: 7, 871: 2, 889: 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, 1595: 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 diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c index f21fac8f0..9117154b0 100644 --- a/selfdrive/common/touch.c +++ b/selfdrive/common/touch.c @@ -90,3 +90,31 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) { return up; } +int touch_read(TouchState *s, int* out_x, int* out_y) { + assert(out_x && out_y); + struct input_event event; + int err = read(s->fd, &event, sizeof(event)); + if (err < sizeof(event)) { + return -1; + } + bool up = false; + switch (event.type) { + case EV_ABS: + if (event.code == ABS_MT_POSITION_X) { + s->last_x = event.value; + } else if (event.code == ABS_MT_POSITION_Y) { + s->last_y = event.value; + } + up = true; + break; + default: + break; + } + if (up) { + // adjust for flippening + *out_x = s->last_y; + *out_y = 1080 - s->last_x; + } + return up; +} + diff --git a/selfdrive/common/touch.h b/selfdrive/common/touch.h index c2bb6dfec..c48f66b98 100644 --- a/selfdrive/common/touch.h +++ b/selfdrive/common/touch.h @@ -12,6 +12,7 @@ typedef struct TouchState { void touch_init(TouchState *s); int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout); +int touch_read(TouchState *s, int* out_x, int* out_y); #ifdef __cplusplus } diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index b1cb55d12..fd8219990 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.5.10-release" +#define COMMA_VERSION "0.5.11-release" diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc index 8179166e1..a533acb59 100644 --- a/selfdrive/common/visionimg.cc +++ b/selfdrive/common/visionimg.cc @@ -68,7 +68,7 @@ VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) { #ifdef QCOM -EGLClientBuffer visionimg_to_egl(const VisionImg *img) { +EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph) { assert((img->size % img->stride) == 0); assert((img->stride % img->bpp) == 0); @@ -87,13 +87,14 @@ EGLClientBuffer visionimg_to_egl(const VisionImg *img) { GraphicBuffer* gb = new GraphicBuffer(img->width, img->height, (PixelFormat)format, GraphicBuffer::USAGE_HW_TEXTURE, img->stride/img->bpp, hnd, false); - + // GraphicBuffer is ref counted by EGLClientBuffer(ANativeWindowBuffer), no need and not possible to release. + *pph = hnd; return (EGLClientBuffer) gb->getNativeBuffer(); } -GLuint visionimg_to_gl(const VisionImg *img) { +GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) { - EGLClientBuffer buf = visionimg_to_egl(img); + EGLClientBuffer buf = visionimg_to_egl(img, pph); EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); assert(display != EGL_NO_DISPLAY); @@ -107,8 +108,15 @@ GLuint visionimg_to_gl(const VisionImg *img) { glGenTextures(1, &tex); glBindTexture(GL_TEXTURE_2D, tex); glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image); - + *pkhr = image; return tex; } +void visionimg_destroy_gl(EGLImageKHR khr, void *ph) { + EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); + assert(display != EGL_NO_DISPLAY); + eglDestroyImageKHR(display, khr); + delete (private_handle_t*)ph; +} + #endif diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h index 9fabb6054..74b0f3137 100644 --- a/selfdrive/common/visionimg.h +++ b/selfdrive/common/visionimg.h @@ -27,8 +27,9 @@ void visionimg_compute_aligned_width_and_height(int width, int height, int *alig VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf); #ifdef QCOM -EGLClientBuffer visionimg_to_egl(const VisionImg *img); -GLuint visionimg_to_gl(const VisionImg *img); +EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph); +GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph); +void visionimg_destroy_gl(EGLImageKHR khr, void *ph); #endif #ifdef __cplusplus diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b5d699bda..31dcbef07 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,6 +11,7 @@ import selfdrive.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.car.car_helpers import get_car +from selfdrive.controls.lib.model_parser import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \ get_events, \ create_event, \ @@ -72,7 +73,7 @@ def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, if td is not None: overtemp = td.thermal.thermalStatus >= ThermalStatus.red free_space = td.thermal.freeSpace < 0.07 # under 7% of space free no enable allowed - low_battery = td.thermal.batteryPercent < 1 # at zero percent battery, OP should not be allowed + low_battery = td.thermal.batteryPercent < 1 and td.thermal.chargingError # at zero percent battery, while discharging, OP should not be allowed # Create events for battery, temperature and disk space if low_battery: @@ -282,7 +283,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, AM, driver_status, - LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc): + LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc): """Send actuators and hud commands to the car, send live100 and MPC logging""" plan_ts = plan.logMonoTime plan = plan.plan @@ -305,8 +306,21 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = plan.hasLead - CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5) - CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5) + + right_lane_visible = path_plan.pathPlan.rProb > 0.5 + left_lane_visible = path_plan.pathPlan.lProb > 0.5 + + CC.hudControl.rightLaneVisible = bool(right_lane_visible) + CC.hudControl.leftLaneVisible = bool(left_lane_visible) + + blinker = CS.leftBlinker or CS.rightBlinker + ldw_allowed = CS.vEgo > 12.5 and not blinker + + if len(list(path_plan.pathPlan.rPoly)) == 4: + CC.hudControl.rightLaneDepart = bool(ldw_allowed and path_plan.pathPlan.rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible) + if len(list(path_plan.pathPlan.lPoly)) == 4: + CC.hudControl.leftLaneDepart = bool(ldw_allowed and path_plan.pathPlan.lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible) + CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.audibleAlert = AM.audible_alert @@ -327,7 +341,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis "alertType": AM.alert_type, "alertSound": "", # no EON sounds yet "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, - "driverMonitoringOn": bool(driver_status.monitor_on), + "driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": plan_ts, "pathPlanMonoTime": path_plan.logMonoTime, @@ -377,9 +391,6 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis cc_send.carControl = CC carcontrol.send(cc_send.to_bytes()) - if (rk.frame % 36000) == 0: # update angle offset every 6 minutes - params.put("ControlsParams", json.dumps({'angle_model_bias': angle_model_bias})) - return CC @@ -512,7 +523,7 @@ def controlsd_thread(gctx=None, rate=100): # Publish data CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, - live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc) + live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc) prof.checkpoint("Sent") rk.keep_time() # Run at 100Hz diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 10e0e0be6..9af3cb1b1 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -4,26 +4,42 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from common.filter_simple import FirstOrderFilter _DT = 0.01 # update runs at 100Hz -_DTM = 0.1 # DM runs at 10Hz -_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status -_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration -_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car +_DTM = 0.1 # DM runs at 10Hz +_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status +_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration +_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car _DISTRACTED_TIME = 7. _DISTRACTED_PRE_TIME = 4. _DISTRACTED_PROMPT_TIME = 2. -# measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model -_CAMERA_FOV_X = 1. # rad -_CAMERA_FOV_Y = 0.75 # 4/3 aspect ratio # model output refers to center of cropped image, so need to apply the x displacement offset -_CAMERA_OFFSET_X = 0.3125 #(1152/2 - 0.5*(160*864/320))/1152 -_CAMERA_X_CONV = 0.375 # 160*864/320/1152 _PITCH_WEIGHT = 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 -_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch -_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up -_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid -_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz -_VARIANCE_FILTER_TS = 20. # 0.008Hz +_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch +_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up +_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid +_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz +_VARIANCE_FILTER_TS = 20. # 0.008Hz + +RESIZED_FOCAL = 320.0 +H, W, FULL_W = 320, 160, 426 + + +def head_orientation_from_descriptor(desc): + # the output of these angles are in device frame + # so from driver's perspective, pitch is up and yaw is right + # TODO this should be calibrated + pitch_prnet = desc[0] + yaw_prnet = desc[1] + roll_prnet = desc[2] + + face_pixel_position = ((desc[3] + .5)*W - W + FULL_W, (desc[4]+.5)*H) + yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL) + pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL) + + roll = roll_prnet + pitch = pitch_prnet + pitch_focal_angle + yaw = -yaw_prnet + yaw_focal_angle + return np.array([roll, pitch, yaw]) class _DriverPose(): @@ -34,10 +50,12 @@ class _DriverPose(): self.yaw_offset = 0. self.pitch_offset = 0. -def _monitor_hysteresys(variance_level, monitor_valid_prev): + +def _monitor_hysteresis(variance_level, monitor_valid_prev): var_thr = 0.63 if monitor_valid_prev else 0.37 return variance_level < var_thr + class DriverStatus(): def __init__(self, monitor_on=False): self.pose = _DriverPose() @@ -50,6 +68,7 @@ class DriverStatus(): self.variance_high = False self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, _DTM) self.ts_last_check = 0. + self.face_detected = False self._set_timers() def _reset_filters(self): @@ -69,8 +88,8 @@ class DriverStatus(): def _is_driver_distracted(self, pose): # to be tuned and to learn the driver's normal pose - yaw_error = pose.yaw - pose.yaw_offset - pitch_error = pose.pitch - pose.pitch_offset - _PITCH_NATURAL_OFFSET + pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET + yaw_error = pose.yaw # add positive pitch allowance if pitch_error > 0.: pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.) @@ -82,11 +101,14 @@ class DriverStatus(): def get_pose(self, driver_monitoring, params): - self.pose.pitch = driver_monitoring.descriptor[0] - self.pose.yaw = driver_monitoring.descriptor[1] - self.pose.roll = driver_monitoring.descriptor[2] - self.pose.yaw_offset = (driver_monitoring.descriptor[3] * _CAMERA_X_CONV + _CAMERA_OFFSET_X) * _CAMERA_FOV_X - self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down + self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.descriptor) + + # TODO: DM data should not be in a list if they are not homogeneous + if len(driver_monitoring.descriptor) > 6: + self.face_detected = driver_monitoring.descriptor[6] > 0. + else: + self.face_detected = True + self.driver_distracted = self._is_driver_distracted(self.pose) # first order filters self.driver_distraction_filter.update(self.driver_distracted) @@ -102,7 +124,7 @@ class DriverStatus(): self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1" self.ts_last_check = ts - self.monitor_valid = _monitor_hysteresys(self.variance_filter.x, monitor_valid_prev) + self.monitor_valid = _monitor_hysteresis(self.variance_filter.x, monitor_valid_prev) self.monitor_on = self.monitor_valid and self.monitor_param_on if monitor_param_on_prev != self.monitor_param_on: self._reset_filters() @@ -117,7 +139,8 @@ class DriverStatus(): # always reset if driver is in control (unless we are in red alert state) or op isn't active self.awareness = 1. - if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted)) and \ + # only update if face is detected, driver is distracted and distraction filter is high + if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \ not (standstill and self.awareness - self.step_change <= self.threshold_prompt): self.awareness = max(self.awareness - self.step_change, -0.1) @@ -146,4 +169,3 @@ if __name__ == "__main__": print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x) ds.update([], True, True, False) print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x) - diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py index 90abcebf1..c04d820bb 100644 --- a/selfdrive/controls/lib/latcontrol_helpers.py +++ b/selfdrive/controls/lib/latcontrol_helpers.py @@ -59,7 +59,7 @@ def compute_path_pinv(l=50): def model_polyfit(points, path_pinv): - return np.dot(path_pinv, map(float, points)) + return np.dot(path_pinv, [float(x) for x in points]) def calc_desired_path(l_poly, diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 0161d2f94..40628ad09 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -20,7 +20,10 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ class PathPlanner(object): def __init__(self, CP): self.MP = ModelParser() - + + self.l_poly = [0., 0., 0., 0.] + self.r_poly = [0., 0., 0., 0.] + self.last_cloudlog_t = 0 context = zmq.Context() @@ -51,7 +54,8 @@ class PathPlanner(object): angle_steers = CS.carState.steeringAngle active = live100.live100.active - angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage + angle_offset_average = live_parameters.liveParameters.angleOffsetAverage + angle_offset_bias = live100.live100.angleModelBias + angle_offset_average self.MP.update(v_ego, md) @@ -65,7 +69,7 @@ class PathPlanner(object): p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.sR, CP.steerActuatorDelay) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, @@ -84,7 +88,6 @@ class PathPlanner(object): self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias) - # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() @@ -106,16 +109,16 @@ class PathPlanner(object): plan_send = messaging.new_message() plan_send.init('pathPlan') plan_send.pathPlan.laneWidth = float(self.MP.lane_width) - plan_send.pathPlan.dPoly = map(float, self.MP.d_poly) - plan_send.pathPlan.cPoly = map(float, self.MP.c_poly) + plan_send.pathPlan.dPoly = [float(x) for x in self.MP.d_poly] + plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly] plan_send.pathPlan.cProb = float(self.MP.c_prob) - plan_send.pathPlan.lPoly = map(float, l_poly) + plan_send.pathPlan.lPoly = [float(x) for x in l_poly] plan_send.pathPlan.lProb = float(self.MP.l_prob) - plan_send.pathPlan.rPoly = map(float, r_poly) + plan_send.pathPlan.rPoly = [float(x) for x in r_poly] plan_send.pathPlan.rProb = float(self.MP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) - plan_send.pathPlan.angleOffset = float(live_parameters.liveParameters.angleOffsetAverage) + plan_send.pathPlan.angleOffset = float(angle_offset_average) plan_send.pathPlan.valid = bool(plan_valid) plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index c26711f57..1d5b32c8f 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -152,7 +152,7 @@ class Planner(object): # Calculate speed for normal cruise control if enabled: - accel_limits = map(float, calc_cruise_accel_limits(v_ego, following)) + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 4d44c96b9..46d12ae8d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -226,7 +226,7 @@ def radard_thread(gctx=None): if VISION_POINT in ar_pts: print("vision", ar_pts[VISION_POINT]) - idens = tracks.keys() + idens = list(tracks.keys()) track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index d7cbabe63..876b2ab80 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -29,7 +29,7 @@ def can_printer(bus=0, max_msg=None, addr="127.0.0.1"): for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))): if max_msg is None or k < max_msg: dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) - print dd + print(dd) lp = sec_since_boot() if __name__ == "__main__": diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py new file mode 100644 index 000000000..48cca99f9 --- /dev/null +++ b/selfdrive/debug/cpu_usage_stat.py @@ -0,0 +1,99 @@ +import psutil +import time +import os +import sys +import numpy as np +import argparse +import re + +''' +System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs. + Features: + Use psutil library to sample cpu usage(avergage for all cores) of OpenPilot processes, at a rate of 5 samples/sec. + Do cpu usage statistics periodically, 5 seconds as a cycle. + Caculate the average cpu usage within this cycle. + Caculate minumium/maximium/accumulated_average cpu usage as long term inspections. + Monitor multiple processes simuteneously. + Sample usage: + root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd + ('Add monitored proc:', './boardd') + ('Add monitored proc:', 'python locationd/ubloxd.py') + boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96% + ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39% +''' + +# Do statistics every 5 seconds +PRINT_INTERVAL = 5 +SLEEP_INTERVAL = 0.2 + +monitored_proc_names = [ + 'ubloxd', 'thermald', 'uploader', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned', + 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'calibrationd', 'locationd', 'visiond', 'sensord', 'updated', 'gpsd', 'athena'] + + +def get_arg_parser(): + parser = argparse.ArgumentParser( + description="Unlogger and UI", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("proc_names", nargs="?", default='', + help="Process names to be monitored, comma seperated") + parser.add_argument("--list_all", nargs="?", type=bool, default=False, + help="Show all running processes' cmdline") + return parser + + +if __name__ == "__main__": + args = get_arg_parser().parse_args(sys.argv[1:]) + if args.list_all: + for p in psutil.process_iter(): + print('cmdline', p.cmdline(), 'name', p.name()) + sys.exit(0) + + if len(args.proc_names) > 0: + monitored_proc_names = args.proc_names.split(',') + monitored_procs = [] + stats = {} + for p in psutil.process_iter(): + if p == psutil.Process(): + continue + matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])]) + if matched: + k = ' '.join(p.cmdline()) + print('Add monitored proc:', k) + stats[k] = {'cpu_samples': [], 'avg_cpu': None, 'min': None, 'max': None} + monitored_procs.append(p) + i = 0 + interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL) + while True: + for p in monitored_procs: + k = ' '.join(p.cmdline()) + stats[k]['cpu_samples'].append(p.cpu_percent()) + time.sleep(SLEEP_INTERVAL) + i += 1 + if i % interval_int == 0: + l = [] + avg_cpus = [] + for k, stat in stats.items(): + if len(stat['cpu_samples']) <= 0: + continue + avg_cpu = np.array(stat['cpu_samples']).mean() + c = len(stat['cpu_samples']) + stat['cpu_samples'] = [] + if not stat['avg_cpu']: + stat['avg_cpu'] = avg_cpu + else: + stat['avg_cpu'] = (stat['avg_cpu'] * (c + i) + avg_cpu * c) / (c + i + c) + if not stat['min'] or avg_cpu < stat['min']: + stat['min'] = avg_cpu + if not stat['max'] or avg_cpu > stat['max']: + stat['max'] = avg_cpu + msg = 'avg: {1:.2f}%, min: {2:.2f}%, max: {3:.2f}% {0}'.format(os.path.basename(k), stat['avg_cpu'], stat['min'], stat['max']) + l.append((os.path.basename(k), avg_cpu, msg)) + avg_cpus.append(avg_cpu) + l.sort(key= lambda x: -x[1]) + for x in l: + print(x[2]) + print('avg sum: {0:.2f}%\n'.format( + sum([stat['avg_cpu'] for k, stat in stats.items()]) + )) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 45b737953..0434023f7 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -52,7 +52,7 @@ if __name__ == "__main__": server_thread = Thread(target=run_server, args=(socketio,)) server_thread.daemon = True server_thread.start() - print 'server running' + print('server running') values = None if args.values: @@ -68,7 +68,7 @@ if __name__ == "__main__": if sock in republish_socks: republish_socks[sock].send(msg) if args.map and evt.which() == 'liveLocation': - print 'send loc' + print('send loc') socketio.emit('location', { 'lat': evt.liveLocation.lat, 'lon': evt.liveLocation.lon, @@ -83,15 +83,15 @@ if __name__ == "__main__": elif args.json: print(json.loads(msg)) elif args.dump_json: - print json.dumps(evt.to_dict()) + print(json.dumps(evt.to_dict())) elif values: - print "logMonotime = {}".format(evt.logMonoTime) + print("logMonotime = {}".format(evt.logMonoTime)) for value in values: if hasattr(evt, value[0]): item = evt for key in value: item = getattr(item, key) - print "{} = {}".format(".".join(value), item) - print "" + print("{} = {}".format(".".join(value), item)) + print("") else: - print evt + print(evt) diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index dd8eb4d80..c46e84970 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -26,5 +26,5 @@ while True: fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) - print "number of messages:", len(msgs) - print "fingerprint", fingerprint + print("number of messages {0}:".format(len(msgs))) + print("fingerprint {0}".format(fingerprint)) diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/debug/getframes/getframes.py index 292368d6a..496484175 100755 --- a/selfdrive/debug/getframes/getframes.py +++ b/selfdrive/debug/getframes/getframes.py @@ -98,4 +98,4 @@ def getframes(front=False): if __name__ == "__main__": for buf in getframes(): - print buf.shape, buf[101, 101] + print("{0} {1}".format(buf.shape, buf[101, 101])) diff --git a/selfdrive/locationd/Makefile b/selfdrive/locationd/Makefile new file mode 100644 index 000000000..110c1457e --- /dev/null +++ b/selfdrive/locationd/Makefile @@ -0,0 +1,86 @@ +CC = clang +CXX = clang++ + +ARCH := $(shell uname -m) +OS := $(shell uname -o) + +BASEDIR = ../.. +PHONELIBS = ../../phonelibs + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args + +CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) + +ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include +ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \ + -l:libczmq.a -l:libzmq.a \ + -lgnustl_shared + +JSON_FLAGS = -I$(PHONELIBS)/json/src + +EXTRA_LIBS = -lpthread + +ifeq ($(ARCH),x86_64) +ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \ + -l:libczmq.a -l:libzmq.a +endif + +.PHONY: all +all: ubloxd + +include ../common/cereal.mk + +OBJS = ublox_msg.o \ + ubloxd_main.o \ + ../common/swaglog.o \ + ../common/params.o \ + ../common/util.o \ + $(PHONELIBS)/json/src/json.o \ + $(CEREAL_OBJS) + +DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d + +ubloxd: ubloxd.o $(OBJS) + @echo "[ LINK ] $@" + $(CXX) -fPIC -o '$@' $^ \ + $(CEREAL_LIBS) \ + $(ZMQ_LIBS) \ + $(EXTRA_LIBS) + +ubloxd_test: ubloxd_test.o $(OBJS) + @echo "[ LINK ] $@" + $(CXX) -fPIC -o '$@' $^ \ + $(CEREAL_LIBS) \ + $(ZMQ_LIBS) \ + $(EXTRA_LIBS) + +%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) -MMD \ + -Iinclude -I.. -I../.. \ + $(CEREAL_CXXFLAGS) \ + $(ZMQ_FLAGS) \ + $(JSON_FLAGS) \ + -I../ \ + -I../../ \ + -c -o '$@' '$<' + +%.o: %.c + @echo "[ CC ] $@" + $(CC) $(CFLAGS) -MMD \ + -Iinclude -I.. -I../.. \ + $(CEREAL_CFLAGS) \ + $(ZMQ_FLAGS) \ + $(JSON_FLAGS) \ + -c -o '$@' '$<' + +.PHONY: clean +clean: + rm -f ubloxd ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(DEPS) + +-include $(DEPS) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 41911a6b7..61c6e4553 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -23,7 +23,7 @@ VP_INIT = np.array([W/2., H/2.]) # These validity corners were chosen by looking at 1000 # and taking most extreme cases with some margin. -VP_VALIDITY_CORNERS = np.array([[W/2 - 150, 280], [W/2 + 150, 540]]) +VP_VALIDITY_CORNERS = np.array([[W//2 - 150, 280], [W//2 + 150, 540]]) DEBUG = os.getenv("DEBUG") is not None @@ -39,6 +39,7 @@ class Calibrator(object): self.vps = [] self.cal_status = Calibration.UNCALIBRATED self.write_counter = 0 + self.just_calibrated = False self.params = Params() calibration_params = self.params.get("CalibrationParams") if calibration_params: @@ -52,10 +53,16 @@ class Calibrator(object): def update_status(self): + start_status = self.cal_status if len(self.vps) < INPUTS_NEEDED: self.cal_status = Calibration.UNCALIBRATED else: self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID + end_status = self.cal_status + + self.just_calibrated = False + if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED: + self.just_calibrated = True def handle_cam_odom(self, log): trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot @@ -67,7 +74,7 @@ class Calibrator(object): self.vp = np.mean(self.vps, axis=0) self.update_status() self.write_counter += 1 - if self.param_put and self.write_counter % WRITE_CYCLES == 0: + if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated): cal_params = {"vanishing_point": list(self.vp), "valid_points": len(self.vps)} self.params.put("CalibrationParams", json.dumps(cal_params)) @@ -83,10 +90,10 @@ class Calibrator(object): cal_send = messaging.new_message() cal_send.init('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status - cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 / INPUTS_NEEDED, 100) - cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten()) - cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten()) - cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten()) + cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100) + cal_send.liveCalibration.warpMatrix2 = [float(x) for x in warp_matrix.flatten()] + cal_send.liveCalibration.warpMatrixBig = [float(x) for x in warp_matrix_big.flatten()] + cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] livecalibration.send(cal_send.to_bytes()) diff --git a/selfdrive/locationd/kalman/ekf_sym.py b/selfdrive/locationd/kalman/ekf_sym.py index 2dfefe92d..8b537c699 100644 --- a/selfdrive/locationd/kalman/ekf_sym.py +++ b/selfdrive/locationd/kalman/ekf_sym.py @@ -340,7 +340,7 @@ class EKF_sym(object): # rewind if 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) + print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)) return None rewound = self.rewind(t) else: @@ -457,7 +457,7 @@ class EKF_sym(object): # 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' + print('Warning: null space projection failed, measurement ignored') return x, P, np.zeros(A.shape[0] - He.shape[1]) # if using eskf diff --git a/selfdrive/locationd/kalman/kalman_helpers.py b/selfdrive/locationd/kalman/kalman_helpers.py index b15f1542e..ded1fd3fd 100644 --- a/selfdrive/locationd/kalman/kalman_helpers.py +++ b/selfdrive/locationd/kalman/kalman_helpers.py @@ -66,7 +66,7 @@ SAT_OBS = [ObservationKind.PSEUDORANGE_GPS, def run_car_ekf_offline(kf, observations_by_kind): - from laika.raw_gnss import GNSSMeasurement + from laika.raw_gnss import GNSSMeasurement # pylint: disable=import-error observations = [] # create list of observations with element format: [kind, time, data] for kind in observations_by_kind: @@ -131,7 +131,7 @@ def run_observations_through_filter(kf, observations, filter_time=None): def save_residuals_plot(obs, save_path, data_name): import matplotlib.pyplot as plt - import mpld3 + import mpld3 # pylint: disable=import-error fig = plt.figure(figsize=(10,20)) fig.suptitle('Residuals of ' + data_name, fontsize=24) n = len(obs.keys()) diff --git a/selfdrive/locationd/kalman/loc_local_kf.py b/selfdrive/locationd/kalman/loc_local_kf.py index df6d41c77..331b7fee6 100755 --- a/selfdrive/locationd/kalman/loc_local_kf.py +++ b/selfdrive/locationd/kalman/loc_local_kf.py @@ -1,9 +1,9 @@ #!/usr/bin/env python import numpy as np -import loc_local_model +from selfdrive.locationd.kalman import loc_local_model -from kalman_helpers import ObservationKind -from ekf_sym import EKF_sym +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind +from selfdrive.locationd.kalman.ekf_sym import EKF_sym diff --git a/selfdrive/locationd/kalman/loc_local_model.py b/selfdrive/locationd/kalman/loc_local_model.py index 2d69cac02..bc80d9911 100644 --- a/selfdrive/locationd/kalman/loc_local_model.py +++ b/selfdrive/locationd/kalman/loc_local_model.py @@ -2,8 +2,8 @@ import numpy as np import sympy as sp import os -from kalman_helpers import ObservationKind -from ekf_sym import gen_code +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind +from selfdrive.locationd.kalman.ekf_sym import gen_code def gen_model(name, dim_state): diff --git a/selfdrive/locationd/locationd_local.py b/selfdrive/locationd/locationd_local.py index 0b1af2f4c..cc5498cf3 100755 --- a/selfdrive/locationd/locationd_local.py +++ b/selfdrive/locationd/locationd_local.py @@ -73,10 +73,10 @@ class Localizer(object): self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans, log.cameraOdometry.transStd])) - def handle_car_state(self, log, current_time): + def handle_live100(self, log, current_time): self.speed_counter += 1 if self.speed_counter % 5 == 0: - self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo])) + self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.live100.vEgo])) def handle_sensors(self, log, current_time): for sensor_reading in log.sensorEvents: @@ -93,8 +93,8 @@ class Localizer(object): return if typ == "sensorEvents": self.handle_sensors(log, current_time) - elif typ == "carState": - self.handle_car_state(log, current_time) + elif typ == "live100": + self.handle_live100(log, current_time) elif typ == "cameraOdometry": self.handle_cam_odo(log, current_time) @@ -113,7 +113,7 @@ class ParamsLearner(object): self.MAX_SR_TH = MAX_SR_TH * self.VM.sR self.alpha1 = 0.01 * learning_rate - self.alpha2 = 0.00025 * learning_rate + self.alpha2 = 0.0005 * learning_rate self.alpha3 = 0.1 * learning_rate self.alpha4 = 1.0 * learning_rate @@ -154,7 +154,7 @@ class ParamsLearner(object): # instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao)) s5 = "Stiffnes: % .3f x" % self.x - print s4, s5 + print("{0} {1}".format(s4, s5)) self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) @@ -173,7 +173,7 @@ def locationd_thread(gctx, addr, disabled_logs): ctx = zmq.Context() poller = zmq.Poller() - car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True) + live100_socket = messaging.sub_sock(ctx, service_list['live100'].port, poller, addr=addr, conflate=True) sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True) camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True) @@ -219,19 +219,19 @@ def locationd_thread(gctx, addr, disabled_logs): log = messaging.recv_one(socket) localizer.handle_log(log) - if socket is car_state_socket: + if socket is live100_socket: if not localizer.kf.t: continue if i % LEARNING_RATE == 0: - # carState is not updating the Kalman Filter, so update KF manually + # live100 is not updating the Kalman Filter, so update KF manually localizer.kf.predict(1e-9 * log.logMonoTime) predicted_state = localizer.kf.x yaw_rate = -float(predicted_state[5]) - steering_angle = math.radians(log.carState.steeringAngle) - params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle) + steering_angle = math.radians(log.live100.angleSteers) + params_valid = learner.update(yaw_rate, log.live100.vEgo, steering_angle) params = messaging.new_message() params.init('liveParameters') @@ -246,6 +246,7 @@ def locationd_thread(gctx, addr, disabled_logs): params = learner.get_values() params['carFingerprint'] = CP.carFingerprint params_reader.put("LiveParameters", json.dumps(params)) + params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.live100.angleModelBias})) i += 1 elif socket is camera_odometry_socket: @@ -263,7 +264,7 @@ def main(gctx=None, addr="127.0.0.1"): disabled_logs = os.getenv("DISABLED_LOGS", "").split(",") # No speed for now - disabled_logs.append('carState') + disabled_logs.append('live100') if IN_CAR: addr = "192.168.5.11" diff --git a/selfdrive/locationd/test/__init__.py b/selfdrive/locationd/test/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/locationd/test/ci_test.py b/selfdrive/locationd/test/ci_test.py new file mode 100755 index 000000000..7e151093b --- /dev/null +++ b/selfdrive/locationd/test/ci_test.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python2 +import subprocess +import os +import sys +import argparse +import tempfile + +from selfdrive.locationd.test.ubloxd_py_test import parser_test +from selfdrive.locationd.test.ubloxd_regression_test import compare_results + + +def mkdirs_exists_ok(path): + try: + os.makedirs(path) + except OSError: + if not os.path.isdir(path): + raise + + +def main(args): + cur_dir = os.path.dirname(os.path.realpath(__file__)) + ubloxd_dir = os.path.join(cur_dir, '../') + + cc_output_dir = os.path.join(args.output_dir, 'cc') + mkdirs_exists_ok(cc_output_dir) + + py_output_dir = os.path.join(args.output_dir, 'py') + mkdirs_exists_ok(py_output_dir) + + archive_file = os.path.join(cur_dir, args.stream_gz_file) + + try: + print('Extracting stream file') + subprocess.check_call(['tar', 'zxf', archive_file], cwd=tempfile.gettempdir()) + stream_file_path = os.path.join(tempfile.gettempdir(), 'ubloxRaw.stream') + + if not os.path.isfile(stream_file_path): + print('Extract file failed') + sys.exit(-3) + + print('Compiling test app...') + subprocess.check_call(["make", "ubloxd_test"], cwd=ubloxd_dir) + + print('Run regression test - CC parser...') + if args.valgrind: + subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) + else: + subprocess.check_call([os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) + + print('Running regression test - py parser...') + parser_test(stream_file_path, py_output_dir) + + print('Running regression test - compare result...') + r = compare_results(cc_output_dir, py_output_dir) + + print('All done!') + + subprocess.check_call(["rm", stream_file_path]) + subprocess.check_call(["rm", '-rf', cc_output_dir]) + subprocess.check_call(["rm", '-rf', py_output_dir]) + sys.exit(r) + + except subprocess.CalledProcessError as e: + print('CI test failed with {}'.format(e.returncode)) + sys.exit(e.returncode) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Ubloxd CI test", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("stream_gz_file", nargs='?', default='ubloxRaw.tar.gz', + help="UbloxRaw data stream zip file") + + parser.add_argument("output_dir", nargs='?', default='out', + help="Output events temp directory") + + parser.add_argument("--valgrind", default=False, action='store_true', + help="Run in valgrind") + + args = parser.parse_args() + main(args) diff --git a/selfdrive/locationd/ephemeris.py b/selfdrive/locationd/test/ephemeris.py similarity index 100% rename from selfdrive/locationd/ephemeris.py rename to selfdrive/locationd/test/ephemeris.py diff --git a/selfdrive/locationd/ublox.py b/selfdrive/locationd/test/ublox.py similarity index 99% rename from selfdrive/locationd/ublox.py rename to selfdrive/locationd/test/ublox.py index 70e2d2fa0..945b05a12 100644 --- a/selfdrive/locationd/ublox.py +++ b/selfdrive/locationd/test/ublox.py @@ -184,7 +184,7 @@ class UBloxAttrDict(dict): raise AttributeError(name) def __setattr__(self, name, value): - if self.__dict__.has_key(name): + if name in self.__dict__: # allow set on normal attributes dict.__setattr__(self, name, value) else: @@ -256,7 +256,7 @@ class UBloxDescriptor: break if self.count_field == '_remaining': - count = len(buf) / struct.calcsize(self.format2) + count = len(buf) // struct.calcsize(self.format2) if count == 0: msg._unpacked = True diff --git a/selfdrive/locationd/ubloxd.py b/selfdrive/locationd/test/ubloxd.py similarity index 98% rename from selfdrive/locationd/ubloxd.py rename to selfdrive/locationd/test/ubloxd.py index 4cc2a2949..001d54136 100755 --- a/selfdrive/locationd/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import os import serial -import ublox +from selfdrive.locationd.test import ublox import time import datetime import struct @@ -11,7 +11,7 @@ from common import realtime import zmq import selfdrive.messaging as messaging from selfdrive.services import service_list -from ephemeris import EphemerisData, GET_FIELD_U +from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U panda = os.getenv("PANDA") is not None # panda directly connected grey = not (os.getenv("EVAL") is not None) # panda through boardd diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py new file mode 100755 index 000000000..96139ab61 --- /dev/null +++ b/selfdrive/locationd/test/ubloxd_easy.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python +import os +from selfdrive.locationd.test import ublox +from common import realtime +from selfdrive.locationd.test.ubloxd import gen_raw, gen_solution +import zmq +import selfdrive.messaging as messaging +from selfdrive.services import service_list + + +unlogger = os.getenv("UNLOGGER") is not None # debug prints + +def main(gctx=None): + context = zmq.Context() + poller = zmq.Poller() + + context = zmq.Context() + gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port) + ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port) + + # ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port, poller) + + # buffer with all the messages that still need to be input into the kalman + while 1: + polld = poller.poll(timeout=1000) + for sock, mode in polld: + if mode != zmq.POLLIN: + continue + logs = messaging.drain_sock(sock) + for log in logs: + buff = log.ubloxRaw + time = log.logMonoTime + msg = ublox.UBloxMessage() + msg.add(buff) + if msg.valid(): + if msg.name() == 'NAV_PVT': + sol = gen_solution(msg) + if unlogger: + sol.logMonoTime = time + else: + sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) + gpsLocationExternal.send(sol.to_bytes()) + elif msg.name() == 'RXM_RAW': + raw = gen_raw(msg) + if unlogger: + raw.logMonoTime = time + else: + raw.logMonoTime = int(realtime.sec_since_boot() * 1e9) + ubloxGnss.send(raw.to_bytes()) + else: + print "INVALID MESSAGE" + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/test/ubloxd_py_test.py b/selfdrive/locationd/test/ubloxd_py_test.py new file mode 100755 index 000000000..810408118 --- /dev/null +++ b/selfdrive/locationd/test/ubloxd_py_test.py @@ -0,0 +1,77 @@ +import sys +import os + +from selfdrive.locationd.test.ublox import UBloxMessage +from selfdrive.locationd.test.ubloxd import gen_solution, gen_raw, gen_nav_data +from common import realtime + + +def mkdirs_exists_ok(path): + try: + os.makedirs(path) + except OSError: + if not os.path.isdir(path): + raise + + +def parser_test(fn, prefix): + nav_frame_buffer = {} + nav_frame_buffer[0] = {} + for i in xrange(1, 33): + nav_frame_buffer[0][i] = {} + + if not os.path.exists(prefix): + print('Prefix invalid') + sys.exit(-1) + + with open(fn, 'rb') as f: + i = 0 + saved_i = 0 + msg = UBloxMessage() + while True: + n = msg.needed_bytes() + b = f.read(n) + if not b: + break + msg.add(b) + if msg.valid(): + i += 1 + if msg.name() == 'NAV_PVT': + sol = gen_solution(msg) + sol.logMonoTime = int(realtime.sec_since_boot() * 1e9) + with open(os.path.join(prefix, str(saved_i)), 'wb') as f1: + f1.write(sol.to_bytes()) + saved_i += 1 + elif msg.name() == 'RXM_RAW': + raw = gen_raw(msg) + raw.logMonoTime = int(realtime.sec_since_boot() * 1e9) + with open(os.path.join(prefix, str(saved_i)), 'wb') as f1: + f1.write(raw.to_bytes()) + saved_i += 1 + elif msg.name() == 'RXM_SFRBX': + nav = gen_nav_data(msg, nav_frame_buffer) + if nav is not None: + nav.logMonoTime = int(realtime.sec_since_boot() * 1e9) + with open(os.path.join(prefix, str(saved_i)), 'wb') as f1: + f1.write(nav.to_bytes()) + saved_i += 1 + + msg = UBloxMessage() + msg.debug_level = 0 + print('Parsed {} msgs'.format(i)) + print('Generated {} cereal events'.format(saved_i)) + + +if __name__ == "__main__": + if len(sys.argv) < 3: + print('Format: ubloxd_py_test.py file_path prefix') + sys.exit(0) + + fn = sys.argv[1] + if not os.path.isfile(fn): + print('File path invalid') + sys.exit(0) + + prefix = sys.argv[2] + mkdirs_exists_ok(prefix) + parser_test(fn, prefix) diff --git a/selfdrive/locationd/test/ubloxd_regression_test.py b/selfdrive/locationd/test/ubloxd_regression_test.py new file mode 100644 index 000000000..94d6b4fe2 --- /dev/null +++ b/selfdrive/locationd/test/ubloxd_regression_test.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +import os +import sys +import argparse + +from cereal import log +from common.basedir import BASEDIR +os.environ['BASEDIR'] = BASEDIR + + +def get_arg_parser(): + parser = argparse.ArgumentParser( + description="Compare two result files", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("dir1", nargs='?', default='/data/ubloxdc', + help="Directory path 1 from which events are loaded") + + parser.add_argument("dir2", nargs='?', default='/data/ubloxdpy', + help="Directory path 2 from which msgs are loaded") + + return parser + + +def read_file(fn): + with open(fn, 'rb') as f: + return f.read() + + +def compare_results(dir1, dir2): + onlyfiles1 = [f for f in os.listdir(dir1) if os.path.isfile(os.path.join(dir1, f))] + onlyfiles1.sort() + + onlyfiles2 = [f for f in os.listdir(dir2) if os.path.isfile(os.path.join(dir2, f))] + onlyfiles2.sort() + + if len(onlyfiles1) != len(onlyfiles2): + print('len mismatch: {} != {}'.format(len(onlyfiles1), len(onlyfiles2))) + return -1 + events1 = [log.Event.from_bytes(read_file(os.path.join(dir1, f))) for f in onlyfiles1] + events2 = [log.Event.from_bytes(read_file(os.path.join(dir2, f))) for f in onlyfiles2] + + for i in range(len(events1)): + if events1[i].which() != events2[i].which(): + print('event {} type mismatch: {} != {}'.format(i, events1[i].which(), events2[i].which())) + return -2 + if events1[i].which() == 'gpsLocationExternal': + old_gps = events1[i].gpsLocationExternal + gps = events2[i].gpsLocationExternal + # print(gps, old_gps) + attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing', + 'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy'] + for attr in attrs: + o = getattr(old_gps, attr) + n = getattr(gps, attr) + if attr == 'vNED': + if len(o) != len(n): + print('Gps vNED len mismatch', o, n) + return -3 + else: + for i in range(len(o)): + if abs(o[i] - n[i]) > 1e-3: + print('Gps vNED mismatch', o, n) + return + elif o != n: + print('Gps mismatch', attr, o, n) + return -4 + elif events1[i].which() == 'ubloxGnss': + old_gnss = events1[i].ubloxGnss + gnss = events2[i].ubloxGnss + if old_gnss.which() == 'measurementReport' and gnss.which() == 'measurementReport': + attrs = ['gpsWeek', 'leapSeconds', 'measurements', 'numMeas', 'rcvTow', 'receiverStatus', 'schema'] + for attr in attrs: + o = getattr(old_gnss.measurementReport, attr) + n = getattr(gnss.measurementReport, attr) + if str(o) != str(n): + print('measurementReport {} mismatched'.format(attr)) + return -5 + if not (str(old_gnss.measurementReport) == str(gnss.measurementReport)): + print('Gnss measurementReport mismatched!') + print('gnss measurementReport old', old_gnss.measurementReport.measurements) + print('gnss measurementReport new', gnss.measurementReport.measurements) + return -6 + elif old_gnss.which() == 'ephemeris' and gnss.which() == 'ephemeris': + if not (str(old_gnss.ephemeris) == str(gnss.ephemeris)): + print('Gnss ephemeris mismatched!') + print('gnss ephemeris old', old_gnss.ephemeris) + print('gnss ephemeris new', gnss.ephemeris) + return -7 + print('All {} events matched!'.format(len(events1))) + return 0 + + +if __name__ == "__main__": + args = get_arg_parser().parse_args(sys.argv[1:]) + compare_results(args.dir1, args.dir2) diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc new file mode 100644 index 000000000..ef523b887 --- /dev/null +++ b/selfdrive/locationd/ublox_msg.cc @@ -0,0 +1,375 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" + +#include "ublox_msg.h" + +#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4]) +#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) + +namespace ublox { + +inline int twos_complement(uint32_t v, uint32_t nb) { + int sign = v >> (nb - 1); + int value = v; + if(sign != 0) + value = value - (1 << nb); + return value; +} + +inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) { + int v = GET_FIELD_U(w, nb, pos); + return twos_complement(v, nb); +} + +class EphemerisData { + public: + EphemerisData(uint8_t svId, subframes_map subframes) { + this->svId = svId; + int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20); + int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6); + int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( + subframes[1][2+5], 8, 22); + + int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6); + int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22); + int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6); + int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8); + + int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6); + int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14); + int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+2], 24, 6); + int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14); + int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6); + int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14); + uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+6], 24, 6); + int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14); + + int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14); + int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+1], 24, 6); + int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14); + int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+3], 24, 6); + int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14); + int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6); + int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6); + int idot = GET_FIELD_S(subframes[3][2+7], 14, 8); + + this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6); + this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6); + this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6); + this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14); + this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8); + + double gpsPi = 3.1415926535898; + + // now form variables in radians, meters and seconds etc + this->Tgd = t_gd * pow(2, -31); + this->A = pow(a_powhalf * pow(2, -19), 2.0); + this->cic = c_ic * pow(2, -29); + this->cis = c_is * pow(2, -29); + this->crc = c_rc * pow(2, -5); + this->crs = c_rs * pow(2, -5); + this->cuc = c_uc * pow(2, -29); + this->cus = c_us * pow(2, -29); + this->deltaN = delta_n * pow(2, -43) * gpsPi; + this->ecc = e * pow(2, -33); + this->i0 = i_0 * pow(2, -31) * gpsPi; + this->idot = idot * pow(2, -43) * gpsPi; + this->M0 = m_0 * pow(2, -31) * gpsPi; + this->omega = w * pow(2, -31) * gpsPi; + this->omega_dot = omega_dot * pow(2, -43) * gpsPi; + this->omega0 = omega_0 * pow(2, -31) * gpsPi; + this->toe = t_oe * pow(2, 4); + + this->toc = t_oc * pow(2, 4); + this->gpsWeek = week_no; + this->af0 = a_f0 * pow(2, -31); + this->af1 = a_f1 * pow(2, -43); + this->af2 = a_f2 * pow(2, -55); + + uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22); + uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22); + this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff)); + this->iode = iode1; + + if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 && + GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 && + GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) { + double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30); + double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27); + double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24); + double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24); + double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11); + double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14); + double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16); + double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16); + this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3; + this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3; + this->ionoCoeffsValid = true; + } else { + this->ionoCoeffsValid = false; + } + } + uint16_t svId; + double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc; + uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo; + double af0, af1, af2; + bool valid; + double ionoAlpha[4], ionoBeta[4]; + bool ionoCoeffsValid; +}; + +UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) { + nav_frame_buffer[0U] = std::map(); + for(int i = 1;i < 33;i++) + nav_frame_buffer[0U][i] = subframes_map(); +} + +inline int UbloxMsgParser::needed_bytes() { + // Msg header incomplete? + if(bytes_in_parse_buf < UBLOX_HEADER_SIZE) + return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; + uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE; + // too much data + if(needed < (uint16_t)bytes_in_parse_buf) + return -1; + return needed - (uint16_t)bytes_in_parse_buf; +} + +inline bool UbloxMsgParser::valid_cheksum() { + uint8_t ck_a = 0, ck_b = 0; + for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) { + ck_a = (ck_a + msg_parse_buf[i]) & 0xFF; + ck_b = (ck_b + ck_a) & 0xFF; + } + if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) { + LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]); + return false; + } + if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) { + LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]); + return false; + } + return true; +} + +inline bool UbloxMsgParser::valid() { + return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE && + needed_bytes() == 0 && + valid_cheksum(); +} + +inline bool UbloxMsgParser::valid_so_far() { + if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) { + //LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]); + return false; + } + if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) { + //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]); + return false; + } + if(needed_bytes() == 0 && !valid()) + return false; + return true; +} + +kj::Array UbloxMsgParser::gen_solution() { + nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gpsLoc = event.initGpsLocationExternal(); + gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); + gpsLoc.setFlags(msg->flags); + gpsLoc.setLatitude(msg->lat * 1e-07); + gpsLoc.setLongitude(msg->lon * 1e-07); + gpsLoc.setAltitude(msg->height * 1e-03); + gpsLoc.setSpeed(msg->gSpeed * 1e-03); + gpsLoc.setBearing(msg->headMot * 1e-5); + gpsLoc.setAccuracy(msg->hAcc * 1e-03); + std::tm timeinfo = std::tm(); + timeinfo.tm_year = msg->year - 1900; + timeinfo.tm_mon = msg->month - 1; + timeinfo.tm_mday = msg->day; + timeinfo.tm_hour = msg->hour; + timeinfo.tm_min = msg->min; + timeinfo.tm_sec = msg->sec; + std::time_t utc_tt = timegm(&timeinfo); + gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); + float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; + kj::ArrayPtr ap(&f[0], sizeof(f) / sizeof(f[0])); + gpsLoc.setVNED(ap); + gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); + gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); + gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05); + return capnp::messageToFlatArray(msg_builder); +} + +inline bool bit_to_bool(uint8_t val, int shifts) { + return (val & (1 << shifts)) ? true : false; +} + +kj::Array UbloxMsgParser::gen_raw() { + rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + if(bytes_in_parse_buf != ( + UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE + )) { + LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); + return kj::Array(); + } + rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)]; + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gnss = event.initUbloxGnss(); + auto mr = gnss.initMeasurementReport(); + mr.setRcvTow(msg->rcvTow); + mr.setGpsWeek(msg->week); + mr.setLeapSeconds(msg->leapS); + mr.setGpsWeek(msg->week); + auto mb = mr.initMeasurements(msg->numMeas); + for(int8_t i = 0; i < msg->numMeas; i++) { + mb[i].setSvId(measurements[i].svId); + mb[i].setSigId(measurements[i].sigId); + mb[i].setPseudorange(measurements[i].prMes); + mb[i].setCarrierCycles(measurements[i].cpMes); + mb[i].setDoppler(measurements[i].doMes); + mb[i].setGnssId(measurements[i].gnssId); + mb[i].setGlonassFrequencyIndex(measurements[i].freqId); + mb[i].setLocktime(measurements[i].locktime); + mb[i].setCno(measurements[i].cno); + mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong + mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15)); + mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong + auto ts = mb[i].initTrackingStatus(); + ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0)); + ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1)); + ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2)); + ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3)); + } + + mr.setNumMeas(msg->numMeas); + auto rs = mr.initReceiverStatus(); + rs.setLeapSecValid(bit_to_bool(msg->recStat, 0)); + rs.setClkReset(bit_to_bool(msg->recStat, 2)); + return capnp::messageToFlatArray(msg_builder); +} + +kj::Array UbloxMsgParser::gen_nav_data() { + rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + if(bytes_in_parse_buf != ( + UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE + )) { + LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); + return kj::Array(); + } + rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)]; + if(msg->gnssId == 0) { + uint8_t subframeId = GET_FIELD_U(measurements[1].dwrd, 3, 8); + std::vector words; + for(int i = 0; i < msg->numWords;i++) + words.push_back(measurements[i].dwrd); + + if(subframeId == 1) { + nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map(); + nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; + } else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end()) + nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; + if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) { + EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]); + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gnss = event.initUbloxGnss(); + auto eph = gnss.initEphemeris(); + eph.setSvId(ephem_data.svId); + eph.setToc(ephem_data.toc); + eph.setGpsWeek(ephem_data.gpsWeek); + eph.setAf0(ephem_data.af0); + eph.setAf1(ephem_data.af1); + eph.setAf2(ephem_data.af2); + eph.setIode(ephem_data.iode); + eph.setCrs(ephem_data.crs); + eph.setDeltaN(ephem_data.deltaN); + eph.setM0(ephem_data.M0); + eph.setCuc(ephem_data.cuc); + eph.setEcc(ephem_data.ecc); + eph.setCus(ephem_data.cus); + eph.setA(ephem_data.A); + eph.setToe(ephem_data.toe); + eph.setCic(ephem_data.cic); + eph.setOmega0(ephem_data.omega0); + eph.setCis(ephem_data.cis); + eph.setI0(ephem_data.i0); + eph.setCrc(ephem_data.crc); + eph.setOmega(ephem_data.omega); + eph.setOmegaDot(ephem_data.omega_dot); + eph.setIDot(ephem_data.idot); + eph.setTgd(ephem_data.Tgd); + eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); + if(ephem_data.ionoCoeffsValid) { + kj::ArrayPtr apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0])); + eph.setIonoAlpha(apa); + kj::ArrayPtr apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0])); + eph.setIonoBeta(apb); + } else { + eph.setIonoAlpha(kj::ArrayPtr()); + eph.setIonoBeta(kj::ArrayPtr()); + } + return capnp::messageToFlatArray(msg_builder); + } + } + return kj::Array(); +} + +bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { + int needed = needed_bytes(); + if(needed > 0) { + bytes_consumed = min((size_t)needed, incoming_data_len ); + // Add data to buffer + memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed); + bytes_in_parse_buf += bytes_consumed; + } else { + bytes_consumed = incoming_data_len; + } + // Validate msg format, detect invalid header and invalid checksum. + while(!valid_so_far() && bytes_in_parse_buf != 0) { + //LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf); + // Corrupted msg, drop a byte. + bytes_in_parse_buf -= 1; + if(bytes_in_parse_buf > 0) + memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf); + } + // There is redundant data at the end of buffer, reset the buffer. + if(needed_bytes() == -1) + bytes_in_parse_buf = 0; + return valid(); +} + +} diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h new file mode 100644 index 000000000..c7860f14c --- /dev/null +++ b/selfdrive/locationd/ublox_msg.h @@ -0,0 +1,149 @@ +#pragma once + +#include + +#define min(x, y) ((x) <= (y) ? (x) : (y)) + +// NAV_PVT +typedef struct __attribute__((packed)) { + uint32_t iTOW; + uint16_t year; + int8_t month; + int8_t day; + int8_t hour; + int8_t min; + int8_t sec; + int8_t valid; + uint32_t tAcc; + int32_t nano; + int8_t fixType; + int8_t flags; + int8_t flags2; + int8_t numSV; + int32_t lon; + int32_t lat; + int32_t height; + int32_t hMSL; + uint32_t hAcc; + uint32_t vAcc; + int32_t velN; + int32_t velE; + int32_t velD; + int32_t gSpeed; + int32_t headMot; + uint32_t sAcc; + uint32_t headAcc; + uint16_t pDOP; + int8_t reserverd1[6]; + int32_t headVeh; + int16_t magDec; + uint16_t magAcc; +} nav_pvt_msg; + +// RXM_RAW +typedef struct __attribute__((packed)) { + double rcvTow; + uint16_t week; + int8_t leapS; + int8_t numMeas; + int8_t recStat; + int8_t reserved1[3]; +} rxm_raw_msg; + +// Extra data count is in numMeas +typedef struct __attribute__((packed)) { + double prMes; + double cpMes; + float doMes; + int8_t gnssId; + int8_t svId; + int8_t sigId; + int8_t freqId; + uint16_t locktime; + int8_t cno; + int8_t prStdev; + int8_t cpStdev; + int8_t doStdev; + int8_t trkStat; + int8_t reserved3; +} rxm_raw_msg_extra; +// RXM_SFRBX +typedef struct __attribute__((packed)) { + int8_t gnssId; + int8_t svid; + int8_t reserved1; + int8_t freqId; + int8_t numWords; + int8_t reserved2; + int8_t version; + int8_t reserved3; +} rxm_sfrbx_msg; + +// Extra data count is in numWords +typedef struct __attribute__((packed)) { + uint32_t dwrd; +} rxm_sfrbx_msg_extra; + +namespace ublox { + // protocol constants + const uint8_t PREAMBLE1 = 0xb5; + const uint8_t PREAMBLE2 = 0x62; + + // message classes + const uint8_t CLASS_NAV = 0x01; + const uint8_t CLASS_RXM = 0x02; + + // NAV messages + const uint8_t MSG_NAV_PVT = 0x7; + + // RXM messages + const uint8_t MSG_RXM_RAW = 0x15; + const uint8_t MSG_RXM_SFRBX = 0x13; + + const int UBLOX_HEADER_SIZE = 6; + const int UBLOX_CHECKSUM_SIZE = 2; + const int UBLOX_MAX_MSG_SIZE = 65536; + + typedef std::map> subframes_map; + + class UbloxMsgParser { + public: + + UbloxMsgParser(); + kj::Array gen_solution(); + kj::Array gen_raw(); + + kj::Array gen_nav_data(); + bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); + inline void reset() {bytes_in_parse_buf = 0;} + inline uint8_t msg_class() { + return msg_parse_buf[2]; + } + + inline uint8_t msg_id() { + return msg_parse_buf[3]; + } + inline int needed_bytes(); + + void hexdump(uint8_t *d, int l) { + for (int i = 0; i < l; i++) { + if (i%0x10 == 0 && i != 0) printf("\n"); + printf("%02X ", d[i]); + } + printf("\n"); + } + private: + inline bool valid_cheksum(); + inline bool valid(); + inline bool valid_so_far(); + + uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE]; + int bytes_in_parse_buf; + std::map> nav_frame_buffer; + }; + +} + +typedef int (*poll_ubloxraw_msg_func)(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg); +typedef int (*send_gps_event_func)(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags); +int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func); diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc new file mode 100644 index 000000000..82853f132 --- /dev/null +++ b/selfdrive/locationd/ubloxd.cc @@ -0,0 +1,45 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" + +#include "ublox_msg.h" + +const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds + +int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) { + int err; + zmq_pollitem_t item = {.socket = subscriber, .events = ZMQ_POLLIN}; + err = zmq_poll (&item, 1, ZMQ_POLL_TIMEOUT); + if(err <= 0) + return err; + return zmq_msg_recv(msg, subscriber, 0); +} + +int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) { + return zmq_send(s, buf, len, flags); +} + +int main() { + return ubloxd_main(poll_ubloxraw_msg, send_gps_event); +} diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc new file mode 100644 index 000000000..1cfb8c5fb --- /dev/null +++ b/selfdrive/locationd/ubloxd_main.cc @@ -0,0 +1,113 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" + +#include "ublox_msg.h" + +volatile int do_exit = 0; // Flag for process exit on signal + +void set_do_exit(int sig) { + do_exit = 1; +} + +using namespace ublox; + +int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) { + LOGW("starting ubloxd"); + signal(SIGINT, (sighandler_t) set_do_exit); + signal(SIGTERM, (sighandler_t) set_do_exit); + + UbloxMsgParser parser; + void *context = zmq_ctx_new(); + void *gpsLocationExternal = zmq_socket(context, ZMQ_PUB); + zmq_bind(gpsLocationExternal, "tcp://*:8032"); + void *ubloxGnss = zmq_socket(context, ZMQ_PUB); + zmq_bind(ubloxGnss, "tcp://*:8033"); + // ubloxRaw = 8042 + void *subscriber = zmq_socket(context, ZMQ_SUB); + zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); + zmq_connect(subscriber, "tcp://127.0.0.1:8042"); + while (!do_exit) { + zmq_msg_t msg; + zmq_msg_init(&msg); + int err = poll_func(gpsLocationExternal, ubloxGnss, subscriber, &msg); + if(err < 0) { + LOGE_100("zmq_poll error %s in %s", strerror(errno ), __FUNCTION__); + break; + } else if(err == 0) { + continue; + } + // format for board, make copy due to alignment issues, will be freed on out of scope + auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + const uint8_t *data = event.getUbloxRaw().begin(); + size_t len = event.getUbloxRaw().size(); + size_t bytes_consumed = 0; + while(bytes_consumed < len && !do_exit) { + size_t bytes_consumed_this_time = 0U; + if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { + // New message available + if(parser.msg_class() == CLASS_NAV) { + if(parser.msg_id() == MSG_NAV_PVT) { + LOGD("MSG_NAV_PVT"); + auto words = parser.gen_solution(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(parser.msg_class(), parser.msg_id(), gpsLocationExternal, bytes.begin(), bytes.size(), 0); + } + } else + LOGW("Unknown nav msg id: 0x%02X", parser.msg_id()); + } else if(parser.msg_class() == CLASS_RXM) { + if(parser.msg_id() == MSG_RXM_RAW) { + LOGD("MSG_RXM_RAW"); + auto words = parser.gen_raw(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0); + } + } else if(parser.msg_id() == MSG_RXM_SFRBX) { + LOGD("MSG_RXM_SFRBX"); + auto words = parser.gen_nav_data(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0); + } + } else + LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); + } else + LOGW("Unknown msg class: 0x%02X", parser.msg_class()); + parser.reset(); + } + bytes_consumed += bytes_consumed_this_time; + } + zmq_msg_close(&msg); + } + zmq_close(subscriber); + zmq_close(gpsLocationExternal); + zmq_close(ubloxGnss); + zmq_ctx_destroy(context); + return 0; +} diff --git a/selfdrive/locationd/ubloxd_test.cc b/selfdrive/locationd/ubloxd_test.cc new file mode 100644 index 000000000..a1395fa59 --- /dev/null +++ b/selfdrive/locationd/ubloxd_test.cc @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "ublox_msg.h" + +using namespace ublox; + +void write_file(std::string fpath, uint8_t *data, int len) { + FILE* f = fopen(fpath.c_str(), "wb"); + if (!f) { + std::cout << "Open " << fpath << " failed" << std::endl; + return; + } + fwrite(data, len, 1, f); + fclose(f); +} + +static size_t len = 0U; +static size_t consumed = 0U; +static uint8_t *data = NULL; +static int save_idx = 0; +static std::string prefix; +static void *gps_sock, *ublox_gnss_sock; + +int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) { + gps_sock = gpsLocationExternal; + ublox_gnss_sock = ubloxGnss; + size_t consuming = min(len - consumed, 128); + if(consumed < len) { + // create message + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto ublox_raw = event.initUbloxRaw(consuming); + memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming); + auto words = capnp::messageToFlatArray(msg_builder); + auto bytes = words.asBytes(); + zmq_msg_init_size (msg, bytes.size()); + memcpy (zmq_msg_data(msg), (void *)bytes.begin(), bytes.size()); + consumed += consuming; + return 1; + } else + return -1; +} + +int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) { + if(msg_cls == CLASS_NAV && msg_id == MSG_NAV_PVT) + assert(s == gps_sock); + else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_RAW) + assert(s == ublox_gnss_sock); + else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_SFRBX) + assert(s == ublox_gnss_sock); + else + assert(0); + write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len); + save_idx ++; + return len; +} + +int main(int argc, char** argv) { + if(argc < 3) { + printf("Format: ubloxd_test stream_file_path save_prefix\n"); + return 0; + } + // Parse 11360 msgs, generate 9452 events + data = (uint8_t *)read_file(argv[1], &len); + if(data == NULL) { + LOGE("Read file %s failed\n", argv[1]); + return -1; + } + prefix = argv[2]; + ubloxd_main(poll_ubloxraw_msg, send_gps_event); + free(data); + printf("Generated %d cereal events\n", save_idx); + if(save_idx != 9452) { + printf("Event count error: %d\n", save_idx); + return -1; + } + return 0; +} diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 23f499602..788a29b70 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index a1b84cafd..9c6dd646a 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -91,7 +91,7 @@ managed_processes = { "controlsd": "selfdrive.controls.controlsd", "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", - "ubloxd": "selfdrive.locationd.ubloxd", + "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), "mapd": "selfdrive.mapd.mapd", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", @@ -391,7 +391,7 @@ def update_apks(): cloudlog.info("installed apks %s" % (str(installed), )) - for app in installed.iterkeys(): + for app in installed.keys(): apk_path = os.path.join(BASEDIR, "apk/"+app+".apk") if not os.path.exists(apk_path): @@ -462,7 +462,7 @@ def main(): # support additional internal only extensions try: import selfdrive.manager_extensions - selfdrive.manager_extensions.register(register_managed_process) + selfdrive.manager_extensions.register(register_managed_process) # pylint: disable=no-member except ImportError: pass diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py index c98225bd7..b03e66d7a 100755 --- a/selfdrive/mapd/default_speeds_generator.py +++ b/selfdrive/mapd/default_speeds_generator.py @@ -1,5 +1,6 @@ #!/usr/bin/env python import json +import six DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" @@ -145,7 +146,7 @@ def main(filename = DEFAULT_OUTPUT_FILENAME): DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") DE.add_rule({"bicycle_road": "yes"}, "30") - + """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ @@ -205,7 +206,7 @@ class Country(Region): def jsonify(self): ret_dict = {} ret_dict[self.name] = {} - for r_name, region in self.regions.iteritems(): + for r_name, region in six.iteritems(self.regions): ret_dict[self.name].update(region.jsonify()) ret_dict[self.name]['Default'] = self.rules return ret_dict diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 85303857a..d3c82bf40 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -17,7 +17,7 @@ except ImportError as e: os.execv(sys.executable, args) DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" -import default_speeds_generator +from selfdrive.mapd import default_speeds_generator default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) import os @@ -33,7 +33,7 @@ from common.params import Params from common.transformations.coordinates import geodetic2ecef from selfdrive.services import service_list import selfdrive.messaging as messaging -from mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points +from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points import selfdrive.crash as crash from selfdrive.version import version, dirty @@ -121,7 +121,7 @@ def query_thread(): query_lock.release() except Exception as e: - print e + print(e) query_lock.acquire() last_query_result = None query_lock.release() @@ -176,7 +176,7 @@ def mapsd_thread(): xs = pnts[:, 0] ys = pnts[:, 1] - road_points = map(float, xs), map(float, ys) + road_points = [float(x) for x in xs], [float(y) for y in ys] if speed < 10: curvature_valid = False @@ -266,8 +266,8 @@ def mapsd_thread(): if road_points is not None: dat.liveMapData.roadX, dat.liveMapData.roadY = road_points if curvature is not None: - dat.liveMapData.roadCurvatureX = map(float, dists) - dat.liveMapData.roadCurvature = map(float, curvature) + dat.liveMapData.roadCurvatureX = [float(x) for x in dists] + dat.liveMapData.roadCurvature = [float(x) for x in curvature] dat.liveMapData.mapValid = map_valid diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index 0a462765e..50dde6ecb 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -1,3 +1,4 @@ +import six import math import json import numpy as np @@ -91,7 +92,7 @@ def geocode_maxspeed(tags, location_info): rule_valid = all( tag_name in tags and tags[tag_name] == value - for tag_name, value in rule['tags'].iteritems() + for tag_name, value in six.iteritems(rule['tags']) ) if rule_valid: max_speed = rule['speed'] @@ -102,7 +103,7 @@ def geocode_maxspeed(tags, location_info): rule_valid = all( tag_name in tags and tags[tag_name] == value - for tag_name, value in rule['tags'].iteritems() + for tag_name, value in six.iteritems(rule['tags']) ) if rule_valid: max_speed = rule['speed'] @@ -191,6 +192,10 @@ class Way: closest_way = way best_score = score + # Normal score is < 5 + if best_score > 50: + return None + return closest_way def __str__(self): diff --git a/selfdrive/orbd/.gitignore b/selfdrive/orbd/.gitignore deleted file mode 100644 index 829780eb5..000000000 --- a/selfdrive/orbd/.gitignore +++ /dev/null @@ -1,8 +0,0 @@ -orbd -orbd_cpu -test/turbocv_profile -test/turbocv_test -dspout/* -dumb_test -bilinear_lut.h -orb_lut.h diff --git a/selfdrive/orbd/Makefile b/selfdrive/orbd/Makefile deleted file mode 100644 index 32e9c6dfa..000000000 --- a/selfdrive/orbd/Makefile +++ /dev/null @@ -1,105 +0,0 @@ -# CPU - -CC = clang -CXX = clang++ - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -JSON_FLAGS = -I$(PHONELIBS)/json/src - -CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I. -CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I. -LDFLAGS = - -# profile -# CXXFLAGS += -DTURBOCV_PROFILE=1 - -PHONELIBS = ../../phonelibs -BASEDIR = ../.. -EXTERNAL = ../../external -PYTHONLIBS = - -UNAME_M := $(shell uname -m) - -ifeq ($(UNAME_M),x86_64) -# computer - -ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include -ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \ - -l:libczmq.a -l:libzmq.a -lpthread - -OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc - -CXXFLAGS += -fopenmp -LDFLAGS += -lomp - -else -# phone -ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include -ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \ - -l:libczmq.a -l:libzmq.a \ - -lgnustl_shared - -OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include -OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so - -endif - -.PHONY: all -all: orbd - -include ../common/cereal.mk - -DEP_OBJS = ../common/visionipc.o ../common/ipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o - -orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(LDFLAGS) \ - $(ZMQ_LIBS) \ - $(CEREAL_LIBS) \ - -L/usr/lib \ - -L/system/vendor/lib64 \ - -ladsprpc \ - -lm -lz -llog - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) \ - $(ZMQ_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -orbd_dsp.o: orbd.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(ZMQ_FLAGS) \ - $(OPENCV_FLAGS) \ - -DDSP \ - -I../ \ - -I../../ \ - -I../../../ \ - -I./include \ - -c -o '$@' '$<' - -freethedsp.o: dsp/freethedsp.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) \ - -c -o '$@' '$<' - -calculator_stub.o: dsp/gen/calculator_stub.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -I./include -c -o '$@' '$<' - --include internal.mk - -.PHONY: clean -clean: - rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h - diff --git a/selfdrive/orbd/dsp/freethedsp.c b/selfdrive/orbd/dsp/freethedsp.c deleted file mode 100644 index 298f4fd83..000000000 --- a/selfdrive/orbd/dsp/freethedsp.c +++ /dev/null @@ -1,119 +0,0 @@ -// freethedsp by geohot -// (because the DSP should be free) -// released under MIT License - -// usage instructions: -// 1. Compile an example from the Qualcomm Hexagon SDK -// 2. Try to run it on your phone -// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat -// ...here is where people would give up before freethedsp -// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program) -// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./' -// 6. OMG THE DSP WORKS -// 7. Be happy. - -// *** patch may have to change for your phone *** - -// this is patching /dsp/fastrpc_shell_0 -// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8" -// patch to return 0xFFFFFFFF from is_test_enabled instead of 0 -// your fastrpc_shell_0 may vary -#define PATCH_ADDR 0x5200c -#define PATCH_OLD "\x40\x3f\x20\x50" -#define PATCH_NEW "\x40\x3f\x00\x5a" -#define PATCH_LEN (sizeof(PATCH_OLD)-1) -#define _BITS_IOCTL_H_ - -// under 100 lines of code begins now -#include -#include -#include -#include -#include - -// ioctl stuff -#define IOC_OUT 0x40000000 /* copy out parameters */ -#define IOC_IN 0x80000000 /* copy in parameters */ -#define IOC_INOUT (IOC_IN|IOC_OUT) -#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */ - -#define _IOC(inout,group,num,len) \ - (inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num)) -#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t)) - -// ion ioctls -#include -#define ION_IOC_MSM_MAGIC 'M' -#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \ - struct ion_flush_data) - -struct ion_flush_data { - ion_user_handle_t handle; - int fd; - void *vaddr; - unsigned int offset; - unsigned int length; -}; - -// fastrpc ioctls -#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init) - -struct fastrpc_ioctl_init { - uint32_t flags; /* one of FASTRPC_INIT_* macros */ - uintptr_t __user file; /* pointer to elf file */ - int32_t filelen; /* elf file length */ - int32_t filefd; /* ION fd for the file */ - uintptr_t __user mem; /* mem for the PD */ - int32_t memlen; /* mem length */ - int32_t memfd; /* ION fd for the mem */ -}; - -int ioctl(int fd, unsigned long request, void *arg) { - static void *handle = NULL; - static int (*orig_ioctl)(int, int, void*); - - if (handle == NULL) { - handle = dlopen("/system/lib64/libc.so", RTLD_LAZY); - assert(handle != NULL); - orig_ioctl = dlsym(handle, "ioctl"); - } - - int ret = orig_ioctl(fd, request, arg); - - // carefully modify this one - if (request == FASTRPC_IOCTL_INIT) { - struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; - - // confirm patch is correct and do the patch - assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0); - memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN); - - // flush cache - int ionfd = open("/dev/ion", O_RDONLY); - assert(ionfd > 0); - - struct ion_fd_data fd_data; - fd_data.fd = init->memfd; - int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data); - assert(ret == 0); - - struct ion_flush_data flush_data; - flush_data.handle = fd_data.handle; - flush_data.vaddr = (void*)init->mem; - flush_data.offset = 0; - flush_data.length = init->memlen; - ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); - assert(ret == 0); - - struct ion_handle_data handle_data; - handle_data.handle = fd_data.handle; - ret = ioctl(ionfd, ION_IOC_FREE, &handle_data); - assert(ret == 0); - - // cleanup - close(ionfd); - } - - return ret; -} - diff --git a/selfdrive/orbd/dsp/gen/calculator.h b/selfdrive/orbd/dsp/gen/calculator.h deleted file mode 100644 index 86a3de671..000000000 --- a/selfdrive/orbd/dsp/gen/calculator.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _CALCULATOR_H -#define _CALCULATOR_H - -#include -typedef uint8_t uint8; -typedef uint32_t uint32; - -#ifndef __QAIC_HEADER -#define __QAIC_HEADER(ff) ff -#endif //__QAIC_HEADER - -#ifndef __QAIC_HEADER_EXPORT -#define __QAIC_HEADER_EXPORT -#endif // __QAIC_HEADER_EXPORT - -#ifndef __QAIC_HEADER_ATTRIBUTE -#define __QAIC_HEADER_ATTRIBUTE -#endif // __QAIC_HEADER_ATTRIBUTE - -#ifndef __QAIC_IMPL -#define __QAIC_IMPL(ff) ff -#endif //__QAIC_IMPL - -#ifndef __QAIC_IMPL_EXPORT -#define __QAIC_IMPL_EXPORT -#endif // __QAIC_IMPL_EXPORT - -#ifndef __QAIC_IMPL_ATTRIBUTE -#define __QAIC_IMPL_ATTRIBUTE -#endif // __QAIC_IMPL_ATTRIBUTE -#ifdef __cplusplus -extern "C" { -#endif -__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE; -__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE; -#ifdef __cplusplus -} -#endif -#endif //_CALCULATOR_H diff --git a/selfdrive/orbd/dsp/gen/calculator_stub.c b/selfdrive/orbd/dsp/gen/calculator_stub.c deleted file mode 100644 index 66e4a0f82..000000000 --- a/selfdrive/orbd/dsp/gen/calculator_stub.c +++ /dev/null @@ -1,613 +0,0 @@ -#ifndef _CALCULATOR_STUB_H -#define _CALCULATOR_STUB_H -#include "calculator.h" - -// remote.h -#include -#include - -typedef uint32_t remote_handle; -typedef uint64_t remote_handle64; - -typedef struct { - void *pv; - size_t nLen; -} remote_buf; - -typedef struct { - int32_t fd; - uint32_t offset; -} remote_dma_handle; - -typedef union { - remote_buf buf; - remote_handle h; - remote_handle64 h64; - remote_dma_handle dma; -} remote_arg; - -int remote_handle_open(const char* name, remote_handle *ph); -int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra); -int remote_handle_close(remote_handle h); - -#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \ - ((((uint32_t) (nAttr) & 0x7) << 29) | \ - (((uint32_t) (nMethod) & 0x1f) << 24) | \ - (((uint32_t) (nIn) & 0xff) << 16) | \ - (((uint32_t) (nOut) & 0xff) << 8) | \ - (((uint32_t) (noIn) & 0x0f) << 4) | \ - ((uint32_t) (noOut) & 0x0f)) - -#ifndef _QAIC_ENV_H -#define _QAIC_ENV_H - -#ifdef __GNUC__ -#ifdef __clang__ -#pragma GCC diagnostic ignored "-Wunknown-pragmas" -#else -#pragma GCC diagnostic ignored "-Wpragmas" -#endif -#pragma GCC diagnostic ignored "-Wuninitialized" -#pragma GCC diagnostic ignored "-Wunused-parameter" -#pragma GCC diagnostic ignored "-Wunused-function" -#endif - -#ifndef _ATTRIBUTE_UNUSED - -#ifdef _WIN32 -#define _ATTRIBUTE_UNUSED -#else -#define _ATTRIBUTE_UNUSED __attribute__ ((unused)) -#endif - -#endif // _ATTRIBUTE_UNUSED - -#ifndef __QAIC_REMOTE -#define __QAIC_REMOTE(ff) ff -#endif //__QAIC_REMOTE - -#ifndef __QAIC_HEADER -#define __QAIC_HEADER(ff) ff -#endif //__QAIC_HEADER - -#ifndef __QAIC_HEADER_EXPORT -#define __QAIC_HEADER_EXPORT -#endif // __QAIC_HEADER_EXPORT - -#ifndef __QAIC_HEADER_ATTRIBUTE -#define __QAIC_HEADER_ATTRIBUTE -#endif // __QAIC_HEADER_ATTRIBUTE - -#ifndef __QAIC_IMPL -#define __QAIC_IMPL(ff) ff -#endif //__QAIC_IMPL - -#ifndef __QAIC_IMPL_EXPORT -#define __QAIC_IMPL_EXPORT -#endif // __QAIC_IMPL_EXPORT - -#ifndef __QAIC_IMPL_ATTRIBUTE -#define __QAIC_IMPL_ATTRIBUTE -#endif // __QAIC_IMPL_ATTRIBUTE - -#ifndef __QAIC_STUB -#define __QAIC_STUB(ff) ff -#endif //__QAIC_STUB - -#ifndef __QAIC_STUB_EXPORT -#define __QAIC_STUB_EXPORT -#endif // __QAIC_STUB_EXPORT - -#ifndef __QAIC_STUB_ATTRIBUTE -#define __QAIC_STUB_ATTRIBUTE -#endif // __QAIC_STUB_ATTRIBUTE - -#ifndef __QAIC_SKEL -#define __QAIC_SKEL(ff) ff -#endif //__QAIC_SKEL__ - -#ifndef __QAIC_SKEL_EXPORT -#define __QAIC_SKEL_EXPORT -#endif // __QAIC_SKEL_EXPORT - -#ifndef __QAIC_SKEL_ATTRIBUTE -#define __QAIC_SKEL_ATTRIBUTE -#endif // __QAIC_SKEL_ATTRIBUTE - -#ifdef __QAIC_DEBUG__ - #ifndef __QAIC_DBG_PRINTF__ - #include - #define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0) - #endif -#else - #define __QAIC_DBG_PRINTF__( ee ) (void)0 -#endif - - -#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof))) - -#define _COPY(dst, dof, src, sof, sz) \ - do {\ - struct __copy { \ - char ar[sz]; \ - };\ - *(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\ - } while (0) - -#define _COPYIF(dst, dof, src, sof, sz) \ - do {\ - if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\ - _COPY(dst, dof, src, sof, sz); \ - } \ - } while (0) - -_ATTRIBUTE_UNUSED -static __inline void _qaic_memmove(void* dst, void* src, int size) { - int i; - for(i = 0; i < size; ++i) { - ((char*)dst)[i] = ((char*)src)[i]; - } -} - -#define _MEMMOVEIF(dst, src, sz) \ - do {\ - if(dst != src) {\ - _qaic_memmove(dst, src, sz);\ - } \ - } while (0) - - -#define _ASSIGN(dst, src, sof) \ - do {\ - dst = OFFSET(src, sof); \ - } while (0) - -#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str)) - -#define AEE_SUCCESS 0 -#define AEE_EOFFSET 0x80000400 -#define AEE_EBADPARM (AEE_EOFFSET + 0x00E) - -#define _TRY(ee, func) \ - do { \ - if (AEE_SUCCESS != ((ee) = func)) {\ - __QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\ - goto ee##bail;\ - } \ - } while (0) - -#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS) - -#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS) - -#ifdef __QAIC_DEBUG__ -#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv)) -#else -#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv)) -#endif - - -#endif // _QAIC_ENV_H - -#ifndef _ALLOCATOR_H -#define _ALLOCATOR_H - -#include -#include - -typedef struct _heap _heap; -struct _heap { - _heap* pPrev; - const char* loc; - uint64_t buf; -}; - -typedef struct _allocator { - _heap* pheap; - uint8_t* stack; - uint8_t* stackEnd; - int nSize; -} _allocator; - -_ATTRIBUTE_UNUSED -static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) { - _heap* pn = 0; - pn = malloc(size + sizeof(_heap) - sizeof(uint64_t)); - if(pn != 0) { - pn->pPrev = *ppa; - pn->loc = loc; - *ppa = pn; - *ppbuf = (void*)&(pn->buf); - return 0; - } else { - return -1; - } -} -#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1)) - -_ATTRIBUTE_UNUSED -static __inline int _allocator_alloc(_allocator* me, - const char* loc, - int size, - unsigned int al, - void** ppbuf) { - if(size < 0) { - return -1; - } else if (size == 0) { - *ppbuf = 0; - return 0; - } - if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) { - *ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al); - me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size; - return 0; - } else { - return _heap_alloc(&me->pheap, loc, size, ppbuf); - } -} - -_ATTRIBUTE_UNUSED -static __inline void _allocator_deinit(_allocator* me) { - _heap* pa = me->pheap; - while(pa != 0) { - _heap* pn = pa; - const char* loc = pn->loc; - (void)loc; - pa = pn->pPrev; - free(pn); - } -} - -_ATTRIBUTE_UNUSED -static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) { - me->stack = stack; - me->stackEnd = stack + stackSize; - me->nSize = stackSize; - me->pheap = 0; -} - - -#endif // _ALLOCATOR_H - -#ifndef SLIM_H -#define SLIM_H - -#include - -//a C data structure for the idl types that can be used to implement -//static and dynamic language bindings fairly efficiently. -// -//the goal is to have a minimal ROM and RAM footprint and without -//doing too many allocations. A good way to package these things seemed -//like the module boundary, so all the idls within one module can share -//all the type references. - - -#define PARAMETER_IN 0x0 -#define PARAMETER_OUT 0x1 -#define PARAMETER_INOUT 0x2 -#define PARAMETER_ROUT 0x3 -#define PARAMETER_INROUT 0x4 - -//the types that we get from idl -#define TYPE_OBJECT 0x0 -#define TYPE_INTERFACE 0x1 -#define TYPE_PRIMITIVE 0x2 -#define TYPE_ENUM 0x3 -#define TYPE_STRING 0x4 -#define TYPE_WSTRING 0x5 -#define TYPE_STRUCTURE 0x6 -#define TYPE_UNION 0x7 -#define TYPE_ARRAY 0x8 -#define TYPE_SEQUENCE 0x9 - -//these require the pack/unpack to recurse -//so it's a hint to those languages that can optimize in cases where -//recursion isn't necessary. -#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE) -#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION) -#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY) -#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE) - - -typedef struct Type Type; - -#define INHERIT_TYPE\ - int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\ - union {\ - struct {\ - const uintptr_t p1;\ - const uintptr_t p2;\ - } _cast;\ - struct {\ - uint32_t iid;\ - uint32_t bNotNil;\ - } object;\ - struct {\ - const Type *arrayType;\ - int32_t nItems;\ - } array;\ - struct {\ - const Type *seqType;\ - int32_t nMaxLen;\ - } seqSimple; \ - struct {\ - uint32_t bFloating;\ - uint32_t bSigned;\ - } prim; \ - const SequenceType* seqComplex;\ - const UnionType *unionType;\ - const StructType *structType;\ - int32_t stringMaxLen;\ - uint8_t bInterfaceNotNil;\ - } param;\ - uint8_t type;\ - uint8_t nativeAlignment\ - -typedef struct UnionType UnionType; -typedef struct StructType StructType; -typedef struct SequenceType SequenceType; -struct Type { - INHERIT_TYPE; -}; - -struct SequenceType { - const Type * seqType; - uint32_t nMaxLen; - uint32_t inSize; - uint32_t routSizePrimIn; - uint32_t routSizePrimROut; -}; - -//byte offset from the start of the case values for -//this unions case value array. it MUST be aligned -//at the alignment requrements for the descriptor -// -//if negative it means that the unions cases are -//simple enumerators, so the value read from the descriptor -//can be used directly to find the correct case -typedef union CaseValuePtr CaseValuePtr; -union CaseValuePtr { - const uint8_t* value8s; - const uint16_t* value16s; - const uint32_t* value32s; - const uint64_t* value64s; -}; - -//these are only used in complex cases -//so I pulled them out of the type definition as references to make -//the type smaller -struct UnionType { - const Type *descriptor; - uint32_t nCases; - const CaseValuePtr caseValues; - const Type * const *cases; - int32_t inSize; - int32_t routSizePrimIn; - int32_t routSizePrimROut; - uint8_t inAlignment; - uint8_t routAlignmentPrimIn; - uint8_t routAlignmentPrimROut; - uint8_t inCaseAlignment; - uint8_t routCaseAlignmentPrimIn; - uint8_t routCaseAlignmentPrimROut; - uint8_t nativeCaseAlignment; - uint8_t bDefaultCase; -}; - -struct StructType { - uint32_t nMembers; - const Type * const *members; - int32_t inSize; - int32_t routSizePrimIn; - int32_t routSizePrimROut; - uint8_t inAlignment; - uint8_t routAlignmentPrimIn; - uint8_t routAlignmentPrimROut; -}; - -typedef struct Parameter Parameter; -struct Parameter { - INHERIT_TYPE; - uint8_t mode; - uint8_t bNotNil; -}; - -#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64)) -#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff) - -typedef struct Method Method; -struct Method { - uint32_t uScalars; //no method index - int32_t primInSize; - int32_t primROutSize; - int maxArgs; - int numParams; - const Parameter * const *params; - uint8_t primInAlignment; - uint8_t primROutAlignment; -}; - -typedef struct Interface Interface; - -struct Interface { - int nMethods; - const Method * const *methodArray; - int nIIds; - const uint32_t *iids; - const uint16_t* methodStringArray; - const uint16_t* methodStrings; - const char* strings; -}; - - -#endif //SLIM_H - - -#ifndef _CALCULATOR_SLIM_H -#define _CALCULATOR_SLIM_H - -// remote.h - -#include - -#ifndef __QAIC_SLIM -#define __QAIC_SLIM(ff) ff -#endif -#ifndef __QAIC_SLIM_EXPORT -#define __QAIC_SLIM_EXPORT -#endif - -static const Type types[1]; -static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}}; -static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}}; -static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))}; -static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}}; -static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])}; -static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0"; -static const uint16_t methodStrings[5] = {0,37,18,32,27}; -static const uint16_t methodStringsArrays[2] = {3,0}; -__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings}; -#endif //_CALCULATOR_SLIM_H -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _const_calculator_handle -#define _const_calculator_handle ((remote_handle)-1) -#endif //_const_calculator_handle - -static void _calculator_pls_dtor(void* data) { - remote_handle* ph = (remote_handle*)data; - if(_const_calculator_handle != *ph) { - (void)__QAIC_REMOTE(remote_handle_close)(*ph); - *ph = _const_calculator_handle; - } -} - -static int _calculator_pls_ctor(void* ctx, void* data) { - remote_handle* ph = (remote_handle*)data; - *ph = _const_calculator_handle; - if(*ph == (remote_handle)-1) { - return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph); - } - return 0; -} - -#if (defined __qdsp6__) || (defined __hexagon__) -#pragma weak adsp_pls_add_lookup -extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); -#pragma weak HAP_pls_add_lookup -extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); - -__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { - remote_handle* ph; - if(adsp_pls_add_lookup) { - if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { - return *ph; - } - return (remote_handle)-1; - } else if(HAP_pls_add_lookup) { - if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { - return *ph; - } - return (remote_handle)-1; - } - return(remote_handle)-1; -} - -#else //__qdsp6__ || __hexagon__ - -uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare); - -#ifdef _WIN32 -#include "Windows.h" -uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { - return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare); -} -#elif __GNUC__ -uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { - return __sync_val_compare_and_swap(puDest, uCompare, uExchange); -} -#endif //_WIN32 - - -__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { - static remote_handle handle = _const_calculator_handle; - if((remote_handle)-1 != handle) { - return handle; - } else { - remote_handle tmp; - int nErr = _calculator_pls_ctor("calculator", (void*)&tmp); - if(nErr) { - return (remote_handle)-1; - } - if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) { - _calculator_pls_dtor(&tmp); - } - return handle; - } -} - -#endif //__qdsp6__ - -__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE { - return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra); -} - -#ifdef __cplusplus -} -#endif - - -#ifdef __cplusplus -extern "C" { -#endif -extern int remote_register_dma_handle(int, uint32_t); -static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) { - int _numIn[1]; - remote_arg _pra[1]; - uint32_t _primROut[1]; - int _nErr = 0; - _numIn[0] = 0; - _pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut; - _pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut); - _TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra)); - _COPY(_rout0, 0, _primROut, 0, 4); - _CATCH(_nErr) {} - return _nErr; -} -__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE { - uint32_t _mid = 0; - return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet); -} -static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) { - int _numIn[1]; - remote_arg _pra[3]; - uint32_t _primIn[2]; - remote_arg* _praIn; - remote_arg* _praROut; - int _nErr = 0; - _numIn[0] = 1; - _pra[0].buf.pv = (void*)_primIn; - _pra[0].buf.nLen = sizeof(_primIn); - _COPY(_primIn, 0, _in0Len, 0, 4); - _praIn = (_pra + 1); - _praIn[0].buf.pv = _in0[0]; - _praIn[0].buf.nLen = (1 * _in0Len[0]); - _COPY(_primIn, 4, _rout1Len, 0, 4); - _praROut = (_praIn + _numIn[0] + 0); - _praROut[0].buf.pv = _rout1[0]; - _praROut[0].buf.nLen = (1 * _rout1Len[0]); - _TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra)); - _CATCH(_nErr) {} - return _nErr; -} -__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE { - uint32_t _mid = 1; - return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen); -} -#ifdef __cplusplus -} -#endif -#endif //_CALCULATOR_STUB_H diff --git a/selfdrive/orbd/dsp/gen/libcalculator_skel.so b/selfdrive/orbd/dsp/gen/libcalculator_skel.so deleted file mode 100755 index e48cab482..000000000 Binary files a/selfdrive/orbd/dsp/gen/libcalculator_skel.so and /dev/null differ diff --git a/selfdrive/orbd/extractor.h b/selfdrive/orbd/extractor.h deleted file mode 100644 index f506cd386..000000000 --- a/selfdrive/orbd/extractor.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef EXTRACTOR_H -#define EXTRACTOR_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -#define ORBD_KEYPOINTS 3000 -#define ORBD_DESCRIPTOR_LENGTH 32 -#define ORBD_HEIGHT 874 -#define ORBD_WIDTH 1164 -#define ORBD_FOCAL 910 - -// matches OrbFeatures from log.capnp -struct orb_features { - // align this - uint16_t n_corners; - uint16_t xy[ORBD_KEYPOINTS][2]; - uint8_t octave[ORBD_KEYPOINTS]; - uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH]; - int16_t matches[ORBD_KEYPOINTS]; -}; - -// forward declare this -struct pyramid; - -// manage the pyramids in extractor.c -void init_gpyrs(); -int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *); -int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *); - -#ifdef __cplusplus -} -#endif - -#endif // EXTRACTOR_H diff --git a/selfdrive/orbd/orbd.cc b/selfdrive/orbd/orbd.cc deleted file mode 100644 index ce0d47aec..000000000 --- a/selfdrive/orbd/orbd.cc +++ /dev/null @@ -1,191 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "common/visionipc.h" -#include "common/swaglog.h" - -#include "extractor.h" - -#ifdef DSP -#include "dsp/gen/calculator.h" -#else -#include "turbocv.h" -#endif - -#include -#include -#include "cereal/gen/cpp/log.capnp.h" - -#ifndef PATH_MAX -#include -#endif - -volatile int do_exit = 0; - -static void set_do_exit(int sig) { - do_exit = 1; -} - -int main(int argc, char *argv[]) { - int err; - setpriority(PRIO_PROCESS, 0, -13); - printf("starting orbd\n"); - -#ifdef DSP - uint32_t test_leet = 0; - char my_path[PATH_MAX+1]; - memset(my_path, 0, sizeof(my_path)); - - ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path)); - assert(len > 5); - my_path[len-5] = '\0'; - LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX); - - char adsp_path[PATH_MAX+1]; - snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path); - assert(putenv(adsp_path) == 0); - - assert(calculator_init(&test_leet) == 0); - assert(test_leet == 0x1337); - LOGW("orbd init complete"); -#else - init_gpyrs(); -#endif - - signal(SIGINT, (sighandler_t) set_do_exit); - signal(SIGTERM, (sighandler_t) set_do_exit); - - void *ctx = zmq_ctx_new(); - - void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB); - assert(orb_features_sock); - zmq_bind(orb_features_sock, "tcp://*:8058"); - - void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB); - assert(orb_features_summary_sock); - zmq_bind(orb_features_summary_sock, "tcp://*:8062"); - - struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features)); - int last_frame_id = 0; - uint64_t frame_count = 0; - - // every other frame - const int RATE = 2; - - VisionStream stream; - while (!do_exit) { - VisionStreamBufs buf_info; - err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); - if (err) { - printf("visionstream connect fail\n"); - usleep(100000); - continue; - } - uint64_t timestamp_last_eof = 0; - while (!do_exit) { - VIPCBuf *buf; - VIPCBufExtra extra; - buf = visionstream_get(&stream, &extra); - if (buf == NULL) { - printf("visionstream get failed\n"); - break; - } - - // every other frame - frame_count++; - if ((frame_count%RATE) != 0) { - continue; - } - - uint64_t start = nanos_since_boot(); -#ifdef DSP - int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features)); -#else - int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features); -#endif - uint64_t end = nanos_since_boot(); - LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id); - assert(ret == 0); - - if (last_frame_id+RATE != extra.frame_id) { - LOGW("dropped frame!"); - } - - last_frame_id = extra.frame_id; - - if (timestamp_last_eof == 0) { - timestamp_last_eof = extra.timestamp_eof; - continue; - } - - int match_count = 0; - - // *** send OrbFeatures *** - { - // create capnp message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto orb_features = event.initOrbFeatures(); - - // set timestamps - orb_features.setTimestampEof(extra.timestamp_eof); - orb_features.setTimestampLastEof(timestamp_last_eof); - - // init descriptors for send - kj::ArrayPtr descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners); - orb_features.setDescriptors(descriptorsPtr); - - auto xs = orb_features.initXs(features->n_corners); - auto ys = orb_features.initYs(features->n_corners); - auto octaves = orb_features.initOctaves(features->n_corners); - auto matches = orb_features.initMatches(features->n_corners); - - // copy out normalized keypoints - for (int i = 0; i < features->n_corners; i++) { - xs.set(i, (features->xy[i][0] * 1.0f - ORBD_WIDTH / 2) / ORBD_FOCAL); - ys.set(i, (features->xy[i][1] * 1.0f - ORBD_HEIGHT / 2) / ORBD_FOCAL); - octaves.set(i, features->octave[i]); - matches.set(i, features->matches[i]); - match_count += features->matches[i] != -1; - } - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0); - } - - // *** send OrbFeaturesSummary *** - - { - // create capnp message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto orb_features_summary = event.initOrbFeaturesSummary(); - - orb_features_summary.setTimestampEof(extra.timestamp_eof); - orb_features_summary.setTimestampLastEof(timestamp_last_eof); - orb_features_summary.setFeatureCount(features->n_corners); - orb_features_summary.setMatchCount(match_count); - orb_features_summary.setComputeNs(end-start); - - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0); - } - - timestamp_last_eof = extra.timestamp_eof; - } - } - visionstream_destroy(&stream); - return 0; -} - diff --git a/selfdrive/orbd/orbd_wrapper.sh b/selfdrive/orbd/orbd_wrapper.sh deleted file mode 100755 index 8ec7443a3..000000000 --- a/selfdrive/orbd/orbd_wrapper.sh +++ /dev/null @@ -1,13 +0,0 @@ -#!/bin/sh -finish() { - echo "exiting orbd" - pkill -SIGINT -P $$ -} - -trap finish EXIT - -while true; do - ./orbd & - wait $! -done - diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 540f9b161..72b5570da 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 40c678c1e..b118040cc 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/swaglog.py b/selfdrive/swaglog.py index 012bb9380..169898637 100644 --- a/selfdrive/swaglog.py +++ b/selfdrive/swaglog.py @@ -22,7 +22,7 @@ class LogMessageHandler(logging.Handler): self.connect() msg = self.format(record).rstrip('\n') - # print "SEND", repr(msg) + # print("SEND".format(repr(msg))) try: self.sock.send(chr(record.levelno)+msg, zmq.NOBLOCK) except zmq.error.Again: diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 1564aac51..7ffa50b6c 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -1,5 +1,5 @@ -from maneuverplots import ManeuverPlot -from plant import Plant +from selfdrive.test.plant.maneuverplots import ManeuverPlot +from selfdrive.test.plant.plant import Plant import numpy as np @@ -8,7 +8,7 @@ class Maneuver(object): # Was tempted to make a builder class self.distance_lead = kwargs.get("initial_distance_lead", 200.0) self.speed = kwargs.get("initial_speed", 0.0) - self.lead_relevancy = kwargs.get("lead_relevancy", 0) + self.lead_relevancy = kwargs.get("lead_relevancy", 0) self.grade_values = kwargs.get("grade_values", [0.0, 0.0]) self.grade_breakpoints = kwargs.get("grade_breakpoints", [0.0, duration]) @@ -37,8 +37,8 @@ class Maneuver(object): while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]: current_button = buttons_sorted[0][0] buttons_sorted = buttons_sorted[1:] - print "current button changed to", current_button - + print("current button changed to {0}".format(current_button)) + grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values) speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values) @@ -46,11 +46,11 @@ class Maneuver(object): if live100: last_live100 = live100[-1] - d_rel = distance_lead - distance if self.lead_relevancy else 200. - v_rel = speed_lead - speed if self.lead_relevancy else 0. + d_rel = distance_lead - distance if self.lead_relevancy else 200. + v_rel = speed_lead - speed if self.lead_relevancy else 0. if last_live100: - # print last_live100 + # print(last_live100) #develop plots plot.add_data( time=plant.current_time(), @@ -64,8 +64,8 @@ class Maneuver(object): jerk_factor=last_live100.jerkFactor, a_target=last_live100.aTarget, fcw=fcw) - - print "maneuver end" + + print("maneuver end") return (None, plot) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 903a7e4f7..ac641e294 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -207,8 +207,8 @@ class Plant(object): lateral_pos_rel = 0. # print at 5hz - if (self.rk.frame%(self.rate/5)) == 0: - print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel) + if (self.rk.frame % (self.rate//5)) == 0: + print("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)) # ******** publish the car ******** vls_tuple = namedtuple('vls', [ diff --git a/selfdrive/test/plant/plant_ui.py b/selfdrive/test/plant/plant_ui.py index d7b4bf185..5fa0ffad2 100755 --- a/selfdrive/test/plant/plant_ui.py +++ b/selfdrive/test/plant/plant_ui.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -import pygame -from plant import Plant +import pygame # pylint: disable=import-error +from selfdrive.test.plant.plant import Plant from selfdrive.car.honda.values import CruiseButtons import numpy as np import selfdrive.messaging as messaging @@ -41,7 +41,7 @@ if __name__ == "__main__": plant = Plant(100, distance_lead = 40.0) control_offset = 2.0 - control_pts = zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10) + control_pts = list(zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10)) def pt_to_car(pt): x,y = pt @@ -73,9 +73,9 @@ if __name__ == "__main__": x.prob = 0.0 x.std = 1.0 - car_pts = map(pt_to_car, control_pts) + car_pts = [pt_to_car(pt) for pt in control_pts] - print car_pts + print(car_pts) car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3) md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist() @@ -90,9 +90,9 @@ if __name__ == "__main__": cary += plant.speed * plant.ts * math.sin(heading) # positive steering angle = steering right - print plant.angle_steer + print(plant.angle_steer) heading += plant.angle_steer * plant.ts - print heading + print(heading) # draw my car display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER)) diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index b1f1a97a2..b247b4ca1 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -53,16 +53,16 @@ for idx1, f1 in enumerate(fingerprints_flat): for idx2, f2 in enumerate(fingerprints_flat): if idx1 < idx2 and not check_fingerprint_consistency(f1, f2): valid = False - print "Those two fingerprints are inconsistent", car_names[idx1], car_names[idx2] - print "" - print ', '.join("%d: %d" % v for v in sorted(f1.items())) - print "" - print ', '.join("%d: %d" % v for v in sorted(f2.items())) - print "" + print("Those two fingerprints are inconsistent {0} {1}".format(car_names[idx1], car_names[idx2])) + print("") + print(', '.join("%d: %d" % v for v in sorted(f1.items()))) + print("") + print(', '.join("%d: %d" % v for v in sorted(f2.items()))) + print("") -print "Found ", len(fingerprints_flat), " individual fingerprints" +print("Found {0} individual fingerprints".format(len(fingerprints_flat))) if not valid or len(fingerprints_flat) == 0: - print "TEST FAILED" + print("TEST FAILED") sys.exit(1) else: - print "TEST SUCESSFUL" + print("TEST SUCESSFUL") diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 0a825e3a3..13770f50a 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -63,25 +63,25 @@ def with_processes(processes): @phone_only @with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd']) def test_logging(): - print "LOGGING IS SET UP" + print("LOGGING IS SET UP") time.sleep(1.0) @phone_only @with_processes(['visiond']) def test_visiond(): - print "VISIOND IS SET UP" + print("VISIOND IS SET UP") time.sleep(5.0) @phone_only @with_processes(['sensord']) def test_sensord(): - print "SENSORS ARE SET UP" + print("SENSORS ARE SET UP") time.sleep(1.0) @phone_only @with_processes(['ui']) def test_ui(): - print "RUNNING UI" + print("RUNNING UI") time.sleep(1.0) # will have one thing to upload if loggerd ran @@ -89,5 +89,5 @@ def test_ui(): @phone_only @with_processes(['uploader']) def test_uploader(): - print "UPLOADER" + print("UPLOADER") time.sleep(10.0) diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index fea5a8968..bacabec29 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -14,7 +14,7 @@ from common.numpy_fast import clip from common.filter_simple import FirstOrderFilter ThermalStatus = log.ThermalData.ThermalStatus -CURRENT_TAU = 2. # 2s time constant +CURRENT_TAU = 15. # 15s time constant def read_tz(x): with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: @@ -46,7 +46,7 @@ def setup_eon_fan(): bus.write_byte_data(0x21, 0x02, 0x2) # needed? bus.write_byte_data(0x21, 0x04, 0x4) # manual override source except IOError: - print "LEON detected" + print("LEON detected") #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") LEON = True bus.close() @@ -290,16 +290,16 @@ def thermald_thread(): started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') - charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled) + #charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled) msg.thermal.chargingDisabled = charging_disabled - msg.thermal.chargingError = current_filter.x > 1.0 # if current is > 1A out, then charger might be off + msg.thermal.chargingError = current_filter.x > 0. # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9*(started_ts or 0)) msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) - print msg + print(msg) # report to server once per minute if (count%60) == 0: diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index c76294e6c..9b90013d1 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -1,7 +1,6 @@ import os import re import time -import uuid import datetime from raven import Client @@ -38,32 +37,68 @@ def report_tombstone(fn, client): if parsed: parsedict = parsed.groupdict() - message = parsedict.get('thread') or '' - message += parsedict.get('signal') or '' - message += parsedict.get('abort') or '' else: parsedict = {} - message = fn+"\n"+dat[:1024] - client.send( - event_id=uuid.uuid4().hex, - timestamp=datetime.datetime.utcfromtimestamp(mtime), - logger='tombstoned', - platform='other', + thread_line = parsedict.get('thread', '') + thread_parsed = re.match(r'pid: (?P\d+), tid: (?P\d+), name: (?P.*) >>> (?P.*) <<<', thread_line) + if thread_parsed: + thread_parseddict = thread_parsed.groupdict() + else: + thread_parseddict = {} + pid = thread_parseddict.get('pid', '') + tid = thread_parseddict.get('tid', '') + name = thread_parseddict.get('name', 'unknown') + cmd = thread_parseddict.get('cmd', 'unknown') + + signal_line = parsedict.get('signal', '') + signal_parsed = re.match(r'signal (?P.*?), code (?P.*?), fault addr (?P.*)\n', signal_line) + if signal_parsed: + signal_parseddict = signal_parsed.groupdict() + else: + signal_parseddict = {} + signal = signal_parseddict.get('signal', 'unknown') + code = signal_parseddict.get('code', 'unknown') + fault_addr = signal_parseddict.get('fault_addr', '') + + abort_line = parsedict.get('abort', '') + + if parsed: + message = 'Process {} ({}) got signal {} code {}'.format(name, cmd, signal, code) + if abort_line: + message += '\n'+abort_line + else: + message = fn+'\n'+dat[:1024] + + + client.captureMessage( + message=message, + date=datetime.datetime.utcfromtimestamp(mtime), + data={ + 'logger':'tombstoned', + 'platform':'other', + }, sdk={'name': 'tombstoned', 'version': '0'}, extra={ + 'fault_addr': fault_addr, + 'abort_msg': abort_line, + 'pid': pid, + 'tid': tid, + 'name':'{} ({})'.format(name, cmd), 'tombstone_fn': fn, 'header': parsedict.get('header'), 'registers': parsedict.get('registers'), 'backtrace': parsedict.get('backtrace'), 'logtail': logtail, - 'version': version, - 'dirty': not bool(os.environ.get('CLEAN')), }, - user={'id': os.environ.get('DONGLE_ID')}, - message=message, + tags={ + 'name':'{} ({})'.format(name, cmd), + 'signal':signal, + 'code':code, + 'fault_addr':fault_addr, + }, ) - cloudlog.error({"tombstone": message}) + cloudlog.error({'tombstone': message}) def main(gctx): @@ -72,6 +107,7 @@ def main(gctx): client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) + client.user_context({'id': os.environ.get('DONGLE_ID')}) while True: now_tombstones = set(get_tombstones()) diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index a78a48cf2..a2d857a28 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -10,8 +10,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=return-type \ -Werror=format-extra-args -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) +CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS) ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \ diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 33e35d381..dea27ab18 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -50,6 +50,8 @@ #define UI_BUF_COUNT 4 //#define DEBUG_TURN +//#define DEBUG_FPS + const int vwp_w = 1920; const int vwp_h = 1080; const int nav_w = 640; @@ -65,7 +67,11 @@ const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; -const int UI_FREQ = 60; // Hz +const int UI_FREQ = 30; // Hz + +const int MODEL_PATH_MAX_VERTICES_CNT = 98; +const int MODEL_LANE_PATH_CNT = 3; +const int TRACK_POINTS_MAX_CNT = 50 * 2; const uint8_t bg_colors[][4] = { [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, @@ -158,6 +164,21 @@ typedef struct UIScene { bool is_playing_alert; } UIScene; +typedef struct { + float x, y; +}vertex_data; + +typedef struct { + vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; + int cnt; +} model_path_vertices_data; + +typedef struct { + vertex_data v[TRACK_POINTS_MAX_CNT]; + int cnt; +} track_vertices_data; + + typedef struct UIState { pthread_mutex_t lock; pthread_cond_t bg_cond; @@ -212,7 +233,11 @@ typedef struct UIState { GLuint frame_program; GLuint frame_texs[UI_BUF_COUNT]; + EGLImageKHR khr[UI_BUF_COUNT]; + void *priv_hnds[UI_BUF_COUNT]; GLuint frame_front_texs[UI_BUF_COUNT]; + EGLImageKHR khr_front[UI_BUF_COUNT]; + void *priv_hnds_front[UI_BUF_COUNT]; GLint frame_pos_loc, frame_texcoord_loc; GLint frame_texture_loc, frame_transform_loc; @@ -251,6 +276,20 @@ typedef struct UIState { bool alert_blinked; float light_sensor; + + int touch_fd; + + // Hints for re-calculations and redrawing + bool model_changed; + bool livempc_or_live20_changed; + + GLuint frame_vao[2], frame_vbo[2], frame_ibo[2]; + mat4 rear_frame_mat, front_frame_mat; + + model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; + + track_vertices_data track_vertices[2]; + } UIState; static int last_brightness = -1; @@ -534,6 +573,58 @@ static void ui_init(UIState *s) { free(value); } } + for(int i = 0; i < 2; i++) { + float x1, x2, y1, y2; + if (i == 1) { + // flip horizontally so it looks like a mirror + x1 = 0.0; + x2 = 1.0; + y1 = 1.0; + y2 = 0.0; + } else { + x1 = 1.0; + x2 = 0.0; + y1 = 1.0; + y2 = 0.0; + } + const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; + const float frame_coords[4][4] = { + {-1.0, -1.0, x2, y1}, //bl + {-1.0, 1.0, x2, y2}, //tl + { 1.0, 1.0, x1, y2}, //tr + { 1.0, -1.0, x1, y1}, //br + }; + + glGenVertexArrays(1,&s->frame_vao[i]); + glBindVertexArray(s->frame_vao[i]); + glGenBuffers(1, &s->frame_vbo[i]); + glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo[i]); + glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); + glEnableVertexAttribArray(s->frame_pos_loc); + glVertexAttribPointer(s->frame_pos_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)0); + glEnableVertexAttribArray(s->frame_texcoord_loc); + glVertexAttribPointer(s->frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); + glGenBuffers(1, &s->frame_ibo[i]); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo[i]); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); + glBindBuffer(GL_ARRAY_BUFFER,0); + glBindVertexArray(0); + } + + s->model_changed = false; + s->livempc_or_live20_changed = false; + + s->front_frame_mat = matmul(device_transform, full_to_wide_frame_transform); + s->rear_frame_mat = matmul(device_transform, frame_transform); + + for(int i = 0;i < UI_BUF_COUNT; i++) { + s->khr[i] = NULL; + s->priv_hnds[i] = NULL; + s->khr_front[i] = NULL; + s->priv_hnds_front[i] = NULL; + } } static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, @@ -699,8 +790,7 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgRestore(s->vg); } -static void ui_draw_lane_line(UIState *s, const float *points, float off, - NVGcolor color, bool is_ghost) { +static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { const UIScene *scene = &s->scene; nvgSave(s->vg); @@ -711,70 +801,36 @@ static void ui_draw_lane_line(UIState *s, const float *points, float off, nvgBeginPath(s->vg); bool started = false; - for (int i=0; i<49; i++) { - float px = (float)i; - float py = points[i] - off; - vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - if (x < 0 || y < 0.) { + for (int i=0; icnt; i++) { + if (pvd->v[i].x < 0 || pvd->v[i].y < 0.) { continue; } if (!started) { - nvgMoveTo(s->vg, x, y); + nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y); started = true; } else { - nvgLineTo(s->vg, x, y); + nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); } } - for (int i=49; i>0; i--) { - float px = (float)i; - float py = is_ghost?(points[i]-off):(points[i]+off); - vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - if (x < 0 || y < 0.) { - continue; - } - nvgLineTo(s->vg, x, y); - } - nvgClosePath(s->vg); nvgFillColor(s->vg, color); nvgFill(s->vg); nvgRestore(s->vg); } -static void ui_draw_lane(UIState *s, const PathData path, NVGcolor color) { - ui_draw_lane_line(s, path.points, 0.025*path.prob, color, false); - float var = min(path.std, 0.7); - color.a /= 4; - ui_draw_lane_line(s, path.points, -var, color, true); - ui_draw_lane_line(s, path.points, var, color, true); -} - -static void ui_draw_track(UIState *s, bool is_mpc) { +static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) { const UIScene *scene = &s->scene; const PathData path = scene->model.path; const float *mpc_x_coords = &scene->mpc_x[0]; const float *mpc_y_coords = &scene->mpc_y[0]; - nvgSave(s->vg); - nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space - nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x - nvgScale(s->vg, 2.0, 2.0); - nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); - nvgBeginPath(s->vg); - bool started = false; float off = is_mpc?0.3:0.5; float lead_d = scene->lead_d_rel*2.; float path_height = is_mpc?(lead_d>5.)?min(lead_d, 25.)-min(lead_d*0.35, 10.):20. :(lead_d>0.)?min(lead_d, 50.)-min(lead_d*0.35, 10.):49.; - + pvd->cnt = 0; // left side up for (int i=0; i<=path_height; i++) { float px, py, mpx; @@ -789,18 +845,12 @@ static void ui_draw_track(UIState *s, bool is_mpc) { vec4 p_car_space = (vec4){{px, py, 0., 1.}}; vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - if (x < 0 || y < 0) { + if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) { continue; } - - if (!started) { - nvgMoveTo(s->vg, x, y); - started = true; - } else { - nvgLineTo(s->vg, x, y); - } + pvd->v[pvd->cnt].x = p_full_frame.v[0]; + pvd->v[pvd->cnt].y = p_full_frame.v[1]; + pvd->cnt += 1; } // right side down @@ -817,13 +867,54 @@ static void ui_draw_track(UIState *s, bool is_mpc) { vec4 p_car_space = (vec4){{px, py, 0., 1.}}; vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - if (x < 0 || y < 0.) { + pvd->v[pvd->cnt].x = p_full_frame.v[0]; + pvd->v[pvd->cnt].y = p_full_frame.v[1]; + pvd->cnt += 1; + } +} + +static void update_all_track_data(UIState *s) { + const UIScene *scene = &s->scene; + // Draw vision path + update_track_data(s, false, &s->track_vertices[0]); + + if (scene->engaged) { + // Draw MPC path when engaged + update_track_data(s, true, &s->track_vertices[1]); + } +} + + +static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { +const UIScene *scene = &s->scene; + const PathData path = scene->model.path; + const float *mpc_x_coords = &scene->mpc_x[0]; + const float *mpc_y_coords = &scene->mpc_y[0]; + + nvgSave(s->vg); + nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x + nvgScale(s->vg, 2.0, 2.0); + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + nvgBeginPath(s->vg); + + bool started = false; + float off = is_mpc?0.3:0.5; + float lead_d = scene->lead_d_rel*2.; + float path_height = is_mpc?(lead_d>5.)?min(lead_d, 25.)-min(lead_d*0.35, 10.):20. + :(lead_d>0.)?min(lead_d, 50.)-min(lead_d*0.35, 10.):49.; + int vi = 0; + for(int i = 0;i < pvd->cnt;i++) { + if (pvd->v[i].x < 0 || pvd->v[i].y < 0) { continue; } - nvgLineTo(s->vg, x, y); + if (!started) { + nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y); + started = true; + } else { + nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); + } } nvgClosePath(s->vg); @@ -860,33 +951,17 @@ static void draw_frame(UIState *s) { float x1, x2, y1, y2; if (s->scene.frontview) { - // flip horizontally so it looks like a mirror - x1 = 0.0; - x2 = 1.0; - y1 = 1.0; - y2 = 0.0; + glBindVertexArray(s->frame_vao[1]); } else { - x1 = 1.0; - x2 = 0.0; - y1 = 1.0; - y2 = 0.0; + glBindVertexArray(s->frame_vao[0]); } - mat4 out_mat; + mat4 *out_mat; if (s->scene.frontview || s->scene.fullview) { - out_mat = matmul(device_transform, full_to_wide_frame_transform); + out_mat = &s->front_frame_mat; } else { - out_mat = matmul(device_transform, frame_transform); + out_mat = &s->rear_frame_mat; } - - const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; - const float frame_coords[4][4] = { - {-1.0, -1.0, x2, y1}, //bl - {-1.0, 1.0, x2, y2}, //tl - { 1.0, 1.0, x1, y2}, //tr - { 1.0, -1.0, x1, y1}, //br - }; - glActiveTexture(GL_TEXTURE0); if (s->scene.frontview && s->cur_vision_front_idx >= 0) { glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]); @@ -895,40 +970,90 @@ static void draw_frame(UIState *s) { } glUseProgram(s->frame_program); - glUniform1i(s->frame_texture_loc, 0); - glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat.v); - - glEnableVertexAttribArray(s->frame_pos_loc); - glVertexAttribPointer(s->frame_pos_loc, 2, GL_FLOAT, GL_FALSE, - sizeof(frame_coords[0]), frame_coords); - - glEnableVertexAttribArray(s->frame_texcoord_loc); - glVertexAttribPointer(s->frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, - sizeof(frame_coords[0]), &frame_coords[0][2]); + glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat->v); assert(glGetError() == GL_NO_ERROR); - glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, &frame_indicies[0]); + glEnableVertexAttribArray(0); + glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void*)0); + glDisableVertexAttribArray(0); + glBindVertexArray(0); +} + +static inline bool valid_frame_pt(UIState *s, float x, float y) { + return x >= 0 && x <= s->rgb_width && y >= 0 && y <= s->rgb_height; + +} +static void update_lane_line_data(UIState *s, const float *points, float off, bool is_ghost, model_path_vertices_data *pvd) { + pvd->cnt = 0; + for (int i = 0; i < MODEL_PATH_MAX_VERTICES_CNT / 2; i++) { + float px = (float)i; + float py = points[i] - off; + const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; + const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) + continue; + pvd->v[pvd->cnt].x = p_full_frame.v[0]; + pvd->v[pvd->cnt].y = p_full_frame.v[1]; + pvd->cnt += 1; + } + for (int i = MODEL_PATH_MAX_VERTICES_CNT / 2; i > 0; i--) { + float px = (float)i; + float py = is_ghost?(points[i]-off):(points[i]+off); + const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; + const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) + continue; + pvd->v[pvd->cnt].x = p_full_frame.v[0]; + pvd->v[pvd->cnt].y = p_full_frame.v[1]; + pvd->cnt += 1; + } +} + +static void update_all_lane_lines_data(UIState *s, const PathData path, model_path_vertices_data *pstart) { + update_lane_line_data(s, path.points, 0.025*path.prob, false, pstart); + float var = min(path.std, 0.7); + update_lane_line_data(s, path.points, -var, true, pstart + 1); + update_lane_line_data(s, path.points, var, true, pstart + 2); +} + +static void ui_draw_lane(UIState *s, const PathData *path, model_path_vertices_data *pstart, NVGcolor color) { + ui_draw_lane_line(s, pstart, color); + float var = min(path->std, 0.7); + color.a /= 4; + ui_draw_lane_line(s, pstart + 1, color); + ui_draw_lane_line(s, pstart + 2, color); } static void ui_draw_vision_lanes(UIState *s) { const UIScene *scene = &s->scene; + model_path_vertices_data *pvd = &s->model_path_vertices[0]; + if(s->model_changed) { + update_all_lane_lines_data(s, scene->model.left_lane, pvd); + update_all_lane_lines_data(s, scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT); + s->model_changed = false; + } // Draw left lane edge ui_draw_lane( - s, scene->model.left_lane, + s, &scene->model.left_lane, + pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob)); // Draw right lane edge ui_draw_lane( - s, scene->model.right_lane, + s, &scene->model.right_lane, + pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob)); + if(s->livempc_or_live20_changed) { + update_all_track_data(s); + s->livempc_or_live20_changed = false; + } // Draw vision path - ui_draw_track(s, false); - + ui_draw_track(s, false, &s->track_vertices[0]); if (scene->engaged) { // Draw MPC path when engaged - ui_draw_track(s, true); + ui_draw_track(s, true, &s->track_vertices[1]); } } @@ -1379,14 +1504,14 @@ static void ui_draw_vision(UIState *s) { glEnable(GL_SCISSOR_TEST); glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h); glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); draw_frame(s); glViewport(0, 0, s->fb_w, s->fb_h); glDisable(GL_SCISSOR_TEST); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glClear(GL_STENCIL_BUFFER_BIT); - + nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); nvgSave(s->vg); @@ -1439,8 +1564,6 @@ static void ui_draw(UIState *s) { nvgEndFrame(s->vg); glDisable(GL_BLEND); } - - assert(glGetError() == GL_NO_ERROR); } static PathData read_path(cereal_ModelData_PathData_ptr pathp) { @@ -1496,7 +1619,10 @@ static void ui_update(UIState *s) { // do this here for now in lieu of a run_on_main_thread event for (int i=0; iframe_texs[i]); + if(s->khr[i] != NULL) { + visionimg_destroy_gl(s->khr[i], s->priv_hnds[i]); + glDeleteTextures(1, &s->frame_texs[i]); + } VisionImg img = { .fd = s->bufs[i].fd, @@ -1507,7 +1633,7 @@ static void ui_update(UIState *s) { .bpp = 3, .size = s->rgb_buf_len, }; - s->frame_texs[i] = visionimg_to_gl(&img); + s->frame_texs[i] = visionimg_to_gl(&img, &s->khr[i], &s->priv_hnds[i]); glBindTexture(GL_TEXTURE_2D, s->frame_texs[i]); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); @@ -1520,7 +1646,10 @@ static void ui_update(UIState *s) { } for (int i=0; iframe_front_texs[i]); + if(s->khr_front[i] != NULL) { + visionimg_destroy_gl(s->khr_front[i], s->priv_hnds_front[i]); + glDeleteTextures(1, &s->frame_front_texs[i]); + } VisionImg img = { .fd = s->front_bufs[i].fd, @@ -1531,7 +1660,7 @@ static void ui_update(UIState *s) { .bpp = 3, .size = s->rgb_front_buf_len, }; - s->frame_front_texs[i] = visionimg_to_gl(&img); + s->frame_front_texs[i] = visionimg_to_gl(&img, &s->khr_front[i], &s->priv_hnds_front[i]); glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[i]); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); @@ -1556,9 +1685,67 @@ static void ui_update(UIState *s) { s->alert_blinked = false; } - // poll for events - while (true) { - zmq_pollitem_t polls[10] = {{0}}; + zmq_pollitem_t polls[9] = {{0}}; + // Wait for next rgb image from visiond + while(true) { + assert(s->ipc_fd >= 0); + polls[0].fd = s->ipc_fd; + polls[0].events = ZMQ_POLLIN; + int ret = zmq_poll(polls, 1, 1000); + if (ret < 0) { + LOGW("poll failed (%d)", ret); + close(s->ipc_fd); + s->ipc_fd = -1; + s->vision_connected = false; + return; + } else if (ret == 0) + continue; + // vision ipc event + VisionPacket rp; + err = vipc_recv(s->ipc_fd, &rp); + if (err <= 0) { + LOGW("vision disconnected"); + close(s->ipc_fd); + s->ipc_fd = -1; + s->vision_connected = false; + return; + } + if (rp.type == VIPC_STREAM_ACQUIRE) { + bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT; + int idx = rp.d.stream_acq.idx; + + int release_idx; + if (front) { + release_idx = s->cur_vision_front_idx; + } else { + release_idx = s->cur_vision_idx; + } + if (release_idx >= 0) { + VisionPacket rep = { + .type = VIPC_STREAM_RELEASE, + .d = { .stream_rel = { + .type = rp.d.stream_acq.type, + .idx = release_idx, + }}, + }; + vipc_send(s->ipc_fd, &rep); + } + + if (front) { + assert(idx < UI_BUF_COUNT); + s->cur_vision_front_idx = idx; + } else { + assert(idx < UI_BUF_COUNT); + s->cur_vision_idx = idx; + // printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]); + } + } else { + assert(false); + } + break; + } + // peek and consume all events in the zmq queue, then return. + while(true) { polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; @@ -1577,22 +1764,14 @@ static void ui_update(UIState *s) { polls[7].events = ZMQ_POLLIN; polls[8].socket = s->plus_sock_raw; // plus_sock should be last polls[8].events = ZMQ_POLLIN; - int num_polls = 9; - if (s->vision_connected) { - assert(s->ipc_fd >= 0); - polls[9].fd = s->ipc_fd; - polls[9].events = ZMQ_POLLIN; - num_polls++; - } - int ret = zmq_poll(polls, num_polls, 0); if (ret < 0) { LOGW("poll failed (%d)", ret); - break; + return; } if (ret == 0) { - break; + return; } if (polls[0].revents || polls[1].revents || polls[2].revents || @@ -1602,52 +1781,8 @@ static void ui_update(UIState *s) { set_awake(s, true); } - if (s->vision_connected && polls[9].revents) { - // vision ipc event - VisionPacket rp; - err = vipc_recv(s->ipc_fd, &rp); - if (err <= 0) { - LOGW("vision disconnected"); - close(s->ipc_fd); - s->ipc_fd = -1; - s->vision_connected = false; - continue; - } - if (rp.type == VIPC_STREAM_ACQUIRE) { - bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT; - int idx = rp.d.stream_acq.idx; - - int release_idx; - if (front) { - release_idx = s->cur_vision_front_idx; - } else { - release_idx = s->cur_vision_idx; - } - if (release_idx >= 0) { - VisionPacket rep = { - .type = VIPC_STREAM_RELEASE, - .d = { .stream_rel = { - .type = rp.d.stream_acq.type, - .idx = release_idx, - }}, - }; - vipc_send(s->ipc_fd, &rep); - } - - if (front) { - assert(idx < UI_BUF_COUNT); - s->cur_vision_front_idx = idx; - } else { - assert(idx < UI_BUF_COUNT); - s->cur_vision_idx = idx; - // printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]); - } - } else { - assert(false); - } - } else if (polls[8].revents) { + if (polls[8].revents) { // plus socket - zmq_msg_t msg; err = zmq_msg_init(&msg); assert(err == 0); @@ -1670,7 +1805,7 @@ static void ui_update(UIState *s) { } } if (which == NULL) { - continue; + return; } zmq_msg_t msg; @@ -1686,7 +1821,7 @@ static void ui_update(UIState *s) { eventp.p = capn_getp(capn_root(&ctx), 0, 1); struct cereal_Event eventd; cereal_read_Event(&eventd, eventp); - + double t = millis_since_boot(); if (eventd.which == cereal_Event_live100) { struct cereal_Live100Data datad; cereal_read_Live100Data(&datad, eventd.live100); @@ -1799,6 +1934,7 @@ static void ui_update(UIState *s) { s->scene.lead_d_rel = leaddatad.dRel; s->scene.lead_y_rel = leaddatad.yRel; s->scene.lead_v_rel = leaddatad.vRel; + s->livempc_or_live20_changed = true; } else if (eventd.which == cereal_Event_liveCalibration) { s->scene.world_objects_visible = true; struct cereal_LiveCalibrationData datad; @@ -1820,6 +1956,7 @@ static void ui_update(UIState *s) { } else if (eventd.which == cereal_Event_model) { s->scene.model_ts = eventd.logMonoTime; s->scene.model = read_model(eventd.model); + s->model_changed = true; } else if (eventd.which == cereal_Event_liveMpc) { struct cereal_LiveMpcData datad; cereal_read_LiveMpcData(&datad, eventd.liveMpc); @@ -1837,6 +1974,7 @@ static void ui_update(UIState *s) { for (int i = 0; i < 50; i++){ s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); } + s->livempc_or_live20_changed = true; } else if (eventd.which == cereal_Event_thermal) { struct cereal_ThermalData datad; cereal_read_ThermalData(&datad, eventd.thermal); @@ -1877,7 +2015,6 @@ static void ui_update(UIState *s) { zmq_msg_close(&msg); } } - } static int vision_subscribe(int fd, VisionPacket *rp, int type) { @@ -2070,6 +2207,7 @@ int main() { TouchState touch = {0}; touch_init(&touch); + s->touch_fd = touch.fd; char* error = NULL; ui_sound_init(&error); @@ -2089,9 +2227,18 @@ int main() { float smooth_brightness = BRIGHTNESS_B; set_volume(s, 0); - +#ifdef DEBUG_FPS + vipc_t1 = millis_since_boot(); + double t1 = millis_since_boot(); + int draws = 0, old_draws = 0; +#endif //DEBUG_FPS while (!do_exit) { bool should_swap = false; + if (!s->vision_connected) { + // Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen. + // Don't hold the lock while sleeping, so that vision_connect_thread have chances to get the lock. + usleep(30 * 1000); + } pthread_mutex_lock(&s->lock); if (EON) { @@ -2106,27 +2253,53 @@ int main() { set_brightness(s, NEO_BRIGHTNESS); } - ui_update(s); - - // awake on any touch - int touch_x = -1, touch_y = -1; - int touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 100); - if (touched == 1) { - // touch event will still happen :( - set_awake(s, true); + if (!s->vision_connected) { + // Car is not started, keep in idle state and awake on touch events + zmq_pollitem_t polls[1] = {{0}}; + polls[0].fd = s->touch_fd; + polls[0].events = ZMQ_POLLIN; + int ret = zmq_poll(polls, 1, 0); + if (ret < 0) + LOGW("poll failed (%d)", ret); + else if (ret > 0) { + // awake on any touch + int touch_x = -1, touch_y = -1; + int touched = touch_read(&touch, &touch_x, &touch_y); + if (touched == 1) { + set_awake(s, true); + } + } + } else { + // Car started, fetch a new rgb image from ipc and peek for zmq events. + ui_update(s); + if(!s->vision_connected) { + // Visiond process is just stopped, force a redraw to make screen blank again. + ui_draw(s); + glFinish(); + should_swap = true; + } } - // manage wakefulness if (s->awake_timeout > 0) { s->awake_timeout--; } else { set_awake(s, false); } - - if (s->awake) { + // Don't waste resources on drawing in case screen is off or car is not started. + if (s->awake && s->vision_connected) { ui_draw(s); glFinish(); should_swap = true; +#ifdef DEBUG_FPS + draws++; + double t2 = millis_since_boot(); + const double interval = 30.; + if(t2 - t1 >= interval * 1000.) { + printf("ui draw fps: %.2f\n",((double)(draws - old_draws)) / interval) ; + t1 = t2; + old_draws = draws; + } +#endif } if (s->volume_timeout > 0) { diff --git a/selfdrive/updated.py b/selfdrive/updated.py index c8efd8f3a..743eb6f58 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -10,15 +10,15 @@ NICE_LOW_PRIORITY = ["nice", "-n", "19"] def main(gctx=None): while True: # try network - r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) - if r: + ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) + if ping_failed: time.sleep(60) continue # download application update try: r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT) - except subprocess.CalledProcessError, e: + except subprocess.CalledProcessError as e: cloudlog.event("git fetch failed", cmd=e.cmd, output=e.output, diff --git a/selfdrive/version.py b/selfdrive/version.py index 8166c662f..d1e263aec 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -1,5 +1,7 @@ import os import subprocess +from selfdrive.swaglog import cloudlog + with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: version = _versionf.read().split('"')[1] @@ -11,10 +13,20 @@ try: else: branch = subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).rstrip() branch = 'origin/' + branch + subprocess.check_call(["git", "update-index", "--refresh"]) #This is needed otherwise touched files might show up as modified dirty = subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0 + if dirty: + dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"]) + commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"]).rstrip() + origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch]).rstrip() + cloudlog.event("dirty comma branch", vesion=version, dirty=dirty, origin=origin, branch=branch, dirty_files=dirty_files, commit=commit, origin_commit=origin_commit) else: dirty = True except subprocess.CalledProcessError: + try: + cloudlog.exception("git subprocess failed while finding version") + except: + pass dirty = True # put this here diff --git a/selfdrive/visiond/build_from_src.mk b/selfdrive/visiond/build_from_src.mk index e23887f27..c9394f34a 100644 --- a/selfdrive/visiond/build_from_src.mk +++ b/selfdrive/visiond/build_from_src.mk @@ -58,7 +58,8 @@ endif OPENCV_FLAGS = OPENCV_LIBS = -lopencv_video \ -lopencv_imgproc \ - -lopencv_core + -lopencv_core \ + -lopencv_highgui OTHER_LIBS = -lz -lm -lpthread PLATFORM_OBJS = camera_fake.o \ @@ -127,6 +128,7 @@ OBJS += $(PLATFORM_OBJS) \ ../common/buffering.o \ transform.o \ loadyuv.o \ + rgb_to_yuv.o \ commonmodel.o \ snpemodel.o \ monitoring.o \ @@ -170,6 +172,16 @@ endif DEPS := $(OBJS:.o=.d) +rgb_to_yuv_test: rgb_to_yuv_test.o clutil.o rgb_to_yuv.o ../common/util.o + @echo "[ LINK ] $@" + $(CXX) -fPIC -o '$@' $^ \ + $(LIBYUV_LIBS) \ + $(LDFLAGS) \ + -L/usr/lib \ + -L/system/vendor/lib64 \ + $(OPENCL_LIBS) \ + + $(OUTPUT): $(OBJS) @echo "[ LINK ] $@" $(CXX) -fPIC -o '$@' $^ \ @@ -222,6 +234,6 @@ $(MODEL_OBJS): %.o: %.dlc .PHONY: clean clean: - rm -f visiond $(OBJS) $(DEPS) + rm -f visiond rgb_to_yuv_test rgb_to_yuv_test.o $(OBJS) $(DEPS) -include $(DEPS) diff --git a/selfdrive/visiond/monitoring.cc b/selfdrive/visiond/monitoring.cc index 84ed9e8c5..921473108 100644 --- a/selfdrive/visiond/monitoring.cc +++ b/selfdrive/visiond/monitoring.cc @@ -41,7 +41,7 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, MonitoringResult ret = {0}; memcpy(ret.vs, s->output, sizeof(ret.vs)); - ret.std = sqrtf(2.f) / s->output[6]; + ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1]; return ret; } diff --git a/selfdrive/visiond/monitoring.h b/selfdrive/visiond/monitoring.h index 5689e7941..1e9e25018 100644 --- a/selfdrive/visiond/monitoring.h +++ b/selfdrive/visiond/monitoring.h @@ -8,10 +8,10 @@ extern "C" { #endif -#define OUTPUT_SIZE 7 +#define OUTPUT_SIZE 8 typedef struct MonitoringResult { - float vs[6]; + float vs[OUTPUT_SIZE - 1]; float std; } MonitoringResult; diff --git a/selfdrive/visiond/rgb_to_yuv.c b/selfdrive/visiond/rgb_to_yuv.c new file mode 100644 index 000000000..0ce3d5a02 --- /dev/null +++ b/selfdrive/visiond/rgb_to_yuv.c @@ -0,0 +1,53 @@ +#include +#include + +#include "clutil.h" + +#include "rgb_to_yuv.h" + +void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { + int err = 0; + memset(s, 0, sizeof(*s)); + assert(width % 2 == 0); + assert(height % 2 == 0); + s->width = width; + s->height = height; + char args[1024]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " +#ifdef CL_DEBUG + "-DCL_DEBUG " +#endif + "-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d", + width, height, width/ 2, height / 2, rgb_stride, width * height); + cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "rgb_to_yuv.cl", args); + + s->rgb_to_yuv_krnl = clCreateKernel(prg, "rgb_to_yuv", &err); + assert(err == 0); + // done with this + err = clReleaseProgram(prg); + assert(err == 0); +} + +void rgb_to_yuv_destroy(RGBToYUVState* s) { + int err = 0; + err = clReleaseKernel(s->rgb_to_yuv_krnl); + assert(err == 0); +} + +void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) { + int err = 0; + err = clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl); + assert(err == 0); + err = clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl); + assert(err == 0); + const size_t work_size[2] = { + (size_t)(s->width + (s->width % 4 == 0 ? 0 : (4 - s->width % 4))) / 4, + (size_t)(s->height + (s->height % 4 == 0 ? 0 : (4 - s->height % 4))) / 4 + }; + cl_event event; + err = clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event); + assert(err == 0); + clWaitForEvents(1, &event); + clReleaseEvent(event); +} diff --git a/selfdrive/visiond/rgb_to_yuv.cl b/selfdrive/visiond/rgb_to_yuv.cl new file mode 100644 index 000000000..60dbdb4d5 --- /dev/null +++ b/selfdrive/visiond/rgb_to_yuv.cl @@ -0,0 +1,127 @@ +#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) +#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) +#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) +#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) + +inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) { + uchar2 yy = (uchar2)( + RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), + RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3) + ); +#ifdef CL_DEBUG + if(yi >= RGB_SIZE) + printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE); +#endif + vstore2(yy, 0, out_yuv + yi); +} + +inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) { + const uchar4 yy = (uchar4)( + RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0), + RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3), + RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6), + RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1) + ); +#ifdef CL_DEBUG + if(yi > RGB_SIZE - 4) + printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4); +#endif + vstore4(yy, 0, out_yuv + yi); +} + +inline void convert_uv(__global uchar * out_yuv, int ui, int vi, + const uchar8 rgbs1, const uchar8 rgbs2) { + // U & V: average of 2x2 pixels square + const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); + const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); + const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); +#ifdef CL_DEBUG + if(ui >= RGB_SIZE + RGB_SIZE / 4) + printf("U overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4); + if(vi >= RGB_SIZE + RGB_SIZE / 2) + printf("V overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2); +#endif + out_yuv[ui] = RGB_TO_U(ar, ag, ab); + out_yuv[vi] = RGB_TO_V(ar, ag, ab); +} + +inline void convert_2_uvs(__global uchar * out_yuv, int ui, int vi, + const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) { + // U & V: average of 2x2 pixels square + const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3); + const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4); + const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5); + const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1); + const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2); + const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3); + uchar2 u2 = (uchar2)( + RGB_TO_U(ar1, ag1, ab1), + RGB_TO_U(ar2, ag2, ab2) + ); + uchar2 v2 = (uchar2)( + RGB_TO_V(ar1, ag1, ab1), + RGB_TO_V(ar2, ag2, ab2) + ); +#ifdef CL_DEBUG1 + if(ui > RGB_SIZE + RGB_SIZE / 4 - 2) + printf("U 2 overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4 - 2); + if(vi > RGB_SIZE + RGB_SIZE / 2 - 2) + printf("V 2 overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2 - 2); +#endif + vstore2(u2, 0, out_yuv + ui); + vstore2(v2, 0, out_yuv + vi); +} + +__kernel void rgb_to_yuv(__global uchar const * const rgb, + __global uchar * out_yuv) +{ + const int dx = get_global_id(0); + const int dy = get_global_id(1); + const int col = mul24(dx, 4); // Current column in rgb image + const int row = mul24(dy, 4); // Current row in rgb image + const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted + const int yi_start = mad24(row, WIDTH, col); // Start offset in the target yuv buffer + int ui = mad24(row / 2, UV_WIDTH, RGB_SIZE + col / 2); + int vi = mad24(row / 2 , UV_WIDTH, RGB_SIZE + UV_WIDTH * UV_HEIGHT + col / 2); + int num_col = min(WIDTH - col, 4); + int num_row = min(HEIGHT - row, 4); + if(num_row == 4) { + const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); + const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); + const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); + const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); + const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2); + const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8); + const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3); + const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8); + if(num_col == 4) { + convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); + convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); + convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1); + convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1); + convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); + convert_2_uvs(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1); + } else if(num_col == 2) { + convert_2_ys(out_yuv, yi_start, rgbs0_0); + convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); + convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0); + convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0); + convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); + convert_uv(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0); + } + } else { + const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start); + const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8); + const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE); + const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8); + if(num_col == 4) { + convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1); + convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1); + convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1); + } else if(num_col == 2) { + convert_2_ys(out_yuv, yi_start, rgbs0_0); + convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0); + convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0); + } + } +} diff --git a/selfdrive/visiond/rgb_to_yuv.h b/selfdrive/visiond/rgb_to_yuv.h new file mode 100644 index 000000000..798958034 --- /dev/null +++ b/selfdrive/visiond/rgb_to_yuv.h @@ -0,0 +1,32 @@ +#ifndef RGB_TO_YUV_H +#define RGB_TO_YUV_H + +#include +#include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + int width, height; + cl_kernel rgb_to_yuv_krnl; +} RGBToYUVState; + +void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride); + +void rgb_to_yuv_destroy(RGBToYUVState* s); + +void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl); + +#ifdef __cplusplus +} +#endif + +#endif // RGB_TO_YUV_H diff --git a/selfdrive/visiond/rgb_to_yuv_test.cc b/selfdrive/visiond/rgb_to_yuv_test.cc new file mode 100644 index 000000000..c8b875105 --- /dev/null +++ b/selfdrive/visiond/rgb_to_yuv_test.cc @@ -0,0 +1,201 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef ANDROID + +#define MAXE 0 +#include + +#else +// The libyuv implementation on ARM is slightly different than on x86 +// Our implementation matches the ARM version, so accept errors of 1 +#define MAXE 1 + +#endif + +#include + +#include + +#include "clutil.h" +#include "rgb_to_yuv.h" + + +static inline double millis_since_boot() { + struct timespec t; + clock_gettime(CLOCK_BOOTTIME, &t); + return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6; +} + +void cl_init(cl_device_id &device_id, cl_context &context) { + int err; + cl_platform_id platform_id = NULL; + cl_uint num_devices; + cl_uint num_platforms; + + err = clGetPlatformIDs(1, &platform_id, &num_platforms); + err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, + &device_id, &num_devices); + cl_print_info(platform_id, device_id); + context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); +} + + +bool compare_results(uint8_t *a, uint8_t *b, int len, int stride, int width, int height, uint8_t *rgb) { + int min_diff = 0., max_diff = 0., max_e = 0.; + int e1 = 0, e0 = 0; + int e0y = 0, e0u = 0, e0v = 0, e1y = 0, e1u = 0, e1v = 0; + int max_e_i = 0; + for (int i = 0;i < len;i++) { + int e = ((int)a[i]) - ((int)b[i]); + if(e < min_diff) { + min_diff = e; + } + if(e > max_diff) { + max_diff = e; + } + int e_abs = std::abs(e); + if(e_abs > max_e) { + max_e = e_abs; + max_e_i = i; + } + if(e_abs < 1) { + e0++; + if(i < stride * height) + e0y++; + else if(i < stride * height + stride * height / 4) + e0u++; + else + e0v++; + } else { + e1++; + if(i < stride * height) + e1y++; + else if(i < stride * height + stride * height / 4) + e1u++; + else + e1v++; + } + } + //printf("max diff : %d, min diff : %d, e < 1: %d, e >= 1: %d\n", max_diff, min_diff, e0, e1); + //printf("Y: e < 1: %d, e >= 1: %d, U: e < 1: %d, e >= 1: %d, V: e < 1: %d, e >= 1: %d\n", e0y, e1y, e0u, e1u, e0v, e1v); + if(max_e <= MAXE) { + return true; + } + int row = max_e_i / stride; + if(row < height) { + printf("max error is Y: %d = (libyuv: %u - cl: %u), row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], row, max_e_i % stride); + } else if(row >= height && row < (height + height / 4)) { + printf("max error is U: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height) / 2, max_e_i % stride / 2); + } else { + printf("max error is V: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height - height / 4) / 2, max_e_i % stride / 2); + } + return false; +} + +int main(int argc, char** argv) { + srand(1337); + + clu_init(); + cl_device_id device_id; + cl_context context; + cl_init(device_id, context) ; + + int err; + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + cl_command_queue q = clCreateCommandQueueWithProperties(context, device_id, props, &err); + if(err != 0) { + std::cout << "clCreateCommandQueueWithProperties error: " << err << std::endl; + } + + int width = 1164; + int height = 874; + + int opt = 0; + while ((opt = getopt(argc, argv, "f")) != -1) + { + switch (opt) + { + case 'f': + std::cout << "Using front camera dimensions" << std::endl; + int width = 1152; + int height = 846; + } + } + + std::cout << "Width: " << width << " Height: " << height << std::endl; + uint8_t *rgb_frame = new uint8_t[width * height * 3]; + + + RGBToYUVState rgb_to_yuv_state; + rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, width, height, width * 3); + + int frame_yuv_buf_size = width * height * 3 / 2; + cl_mem yuv_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err); + uint8_t *frame_yuv_buf = new uint8_t[frame_yuv_buf_size]; + uint8_t *frame_yuv_ptr_y = frame_yuv_buf; + uint8_t *frame_yuv_ptr_u = frame_yuv_buf + (width * height); + uint8_t *frame_yuv_ptr_v = frame_yuv_ptr_u + ((width/2) * (height/2)); + + cl_mem rgb_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err); + int mismatched = 0; + int counter = 0; + srand (time(NULL)); + + for (int i = 0; i < 100; i++){ + for (int i = 0; i < width * height * 3; i++){ + rgb_frame[i] = (uint8_t)rand(); + } + + double t1 = millis_since_boot(); + libyuv::RGB24ToI420((uint8_t*)rgb_frame, width * 3, + frame_yuv_ptr_y, width, + frame_yuv_ptr_u, width/2, + frame_yuv_ptr_v, width/2, + width, height); + double t2 = millis_since_boot(); + //printf("Libyuv: rgb to yuv: %.2fms\n", t2-t1); + + clEnqueueWriteBuffer(q, rgb_cl, CL_TRUE, 0, width * height * 3, (void *)rgb_frame, 0, NULL, NULL); + t1 = millis_since_boot(); + rgb_to_yuv_queue(&rgb_to_yuv_state, q, rgb_cl, yuv_cl); + t2 = millis_since_boot(); + + //printf("OpenCL: rgb to yuv: %.2fms\n", t2-t1); + uint8_t *yyy = (uint8_t *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, + CL_MAP_READ, 0, frame_yuv_buf_size, + 0, NULL, NULL, &err); + if(!compare_results(frame_yuv_ptr_y, yyy, frame_yuv_buf_size, width, width, height, (uint8_t*)rgb_frame)) + mismatched++; + clEnqueueUnmapMemObject(q, yuv_cl, yyy, 0, NULL, NULL); + + // std::this_thread::sleep_for(std::chrono::milliseconds(20)); + if(counter++ % 100 == 0) + printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched); + + } + printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched); + + delete[] frame_yuv_buf; + rgb_to_yuv_destroy(&rgb_to_yuv_state); + clReleaseContext(context); + delete[] rgb_frame; + + if (mismatched == 0) + return 0; + else + return -1; +} diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index af10d760c..f7a52dc0e 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -46,6 +46,7 @@ #include "model.h" #include "monitoring.h" +#include "rgb_to_yuv.h" #include "cereal/gen/cpp/log.capnp.h" @@ -122,6 +123,7 @@ struct VisionState { FrameMetadata yuv_metas[YUV_COUNT]; size_t yuv_buf_size; int yuv_width, yuv_height; + RGBToYUVState rgb_to_yuv_state; // for front camera recording Pool yuv_front_pool; @@ -131,6 +133,7 @@ struct VisionState { FrameMetadata yuv_front_metas[YUV_COUNT]; size_t yuv_front_buf_size; int yuv_front_width, yuv_front_height; + RGBToYUVState front_rgb_to_yuv_state; size_t rgb_buf_size; int rgb_width, rgb_height, rgb_stride; @@ -398,6 +401,10 @@ void init_buffers(VisionState *s) { assert(err == 0); s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err); assert(err == 0); + + rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride); + rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride); + } void free_buffers(VisionState *s) { @@ -831,7 +838,6 @@ void* frontview_thread(void *arg) { if (cnt % 3 == 0) #endif { - // for driver autoexposure, use bottom right corner const int y_start = s->rgb_front_height / 3; const int y_end = s->rgb_front_height; @@ -869,15 +875,9 @@ void* frontview_thread(void *arg) { int yuv_idx = pool_select(&s->yuv_front_pool); s->yuv_front_metas[yuv_idx] = frame_data; - uint8_t *bgr_ptr = (uint8_t*)s->rgb_front_bufs[ui_idx].addr; - libyuv::RGB24ToI420(bgr_ptr, s->rgb_front_stride, - s->yuv_front_bufs[yuv_idx].y, s->yuv_front_width, - s->yuv_front_bufs[yuv_idx].u, s->yuv_front_width/2, - s->yuv_front_bufs[yuv_idx].v, s->yuv_front_width/2, - s->rgb_front_width, s->rgb_front_height); - + rgb_to_yuv_queue(&s->front_rgb_to_yuv_state, q, s->rgb_front_bufs_cl[ui_idx], s->yuv_front_cl[yuv_idx]); + visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); s->yuv_front_metas[yuv_idx] = frame_data; - visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_TO_DEVICE); // no reference required cause we don't use this in visiond //pool_acquire(&s->yuv_front_pool, yuv_idx); @@ -1020,17 +1020,10 @@ void* processing_thread(void *arg) { uint8_t* yuv_ptr_u = s->yuv_bufs[yuv_idx].u; uint8_t* yuv_ptr_v = s->yuv_bufs[yuv_idx].v; cl_mem yuv_cl = s->yuv_cl[yuv_idx]; - - libyuv::RGB24ToI420(bgr_ptr, s->rgb_stride, - yuv_ptr_y, s->yuv_width, - yuv_ptr_u, s->yuv_width/2, - yuv_ptr_v, s->yuv_width/2, - s->rgb_width, s->rgb_height); + rgb_to_yuv_queue(&s->rgb_to_yuv_state, q, s->rgb_bufs_cl[rgb_idx], yuv_cl); + visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); double yt2 = millis_since_boot(); - - visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_TO_DEVICE); - // keep another reference around till were done processing pool_acquire(&s->yuv_pool, yuv_idx); @@ -1129,7 +1122,7 @@ void* processing_thread(void *arg) { //printf("avg %f\n", pose_output[0]); posenet->execute(posenet_input); - + // fix stddevs for (int i = 6; i < 12; i++) { pose_output[i] = log1p(exp(pose_output[i])) + 1e-6;